diff --git a/README.md b/README.md index 5da757c..8c98310 100644 --- a/README.md +++ b/README.md @@ -22,11 +22,11 @@ This package is powered by [NVIDIA Isaac Transport for ROS (NITROS)](https://dev The following table summarizes the per-platform performance statistics of sample graphs that use this package, with links included to the full benchmark output. These benchmark configurations are taken from the [Isaac ROS Benchmark](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark#list-of-isaac-ros-benchmarks) collection, based on the [`ros2_benchmark`](https://github.com/NVIDIA-ISAAC-ROS/ros2_benchmark) framework. -| Sample Graph | Input Size | AGX Orin | Orin NX | Orin Nano 8GB | x86_64 w/ RTX 3060 Ti | -| ----------------------------------------------------------------------------------------------------------------------------------------- | ---------- | --------------------------------------------------------------------------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------- | -| [Rectify Node](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/scripts//isaac_ros_rectify_node.py) | 1080p | [875 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_rectify_node-agx_orin.json)
1.8 ms | [479 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_rectify_node-orin_nx.json)
3.8 ms | [333 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_rectify_node-orin_nano_8gb.json)
4.0 ms | [1540 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_rectify_node-x86_64_rtx_3060Ti.json)
0.37 ms | -| [Stereo Disparity Node](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/scripts//isaac_ros_disparity_node.py) | 1080p | [151 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_disparity_node-agx_orin.json)
9.0 ms | [73.7 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_disparity_node-orin_nx.json)
14 ms | [51.6 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_disparity_node-orin_nano_8gb.json)
22 ms | [451 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_disparity_node-x86_64_rtx_3060Ti.json)
2.9 ms | -| [Stereo Disparity Graph](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/scripts//isaac_ros_disparity_graph.py) | 1080p | [141 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_disparity_graph-agx_orin.json)
10 ms | [72.0 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_disparity_graph-orin_nx.json)
16 ms | [48.7 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_disparity_graph-orin_nano_8gb.json)
24 ms | [425 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_disparity_graph-x86_64_rtx_3060Ti.json)
3.6 ms | +| Sample Graph | Input Size | AGX Orin | Orin NX | Orin Nano 8GB | x86_64 w/ RTX 4060 Ti | +| ----------------------------------------------------------------------------------------------------------------------------------------- | ---------- | --------------------------------------------------------------------------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------ | +| [Rectify Node](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/scripts//isaac_ros_rectify_node.py) | 1080p | [509 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_rectify_node-agx_orin.json)
5.6 ms | [385 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_rectify_node-orin_nx.json)
5.1 ms | [283 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_rectify_node-orin_nano.json)
8.5 ms | [819 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_rectify_node-nuc_4060ti.json)
4.4 ms | +| [Stereo Disparity Node](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/scripts//isaac_ros_disparity_node.py) | 1080p | [158 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_disparity_node-agx_orin.json)
10 ms | [76.8 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_disparity_node-orin_nx.json)
17 ms | [53.5 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_disparity_node-orin_nano.json)
23 ms | [397 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_disparity_node-nuc_4060ti.json)
5.8 ms | +| [Stereo Disparity Graph](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/scripts//isaac_ros_disparity_graph.py) | 1080p | [162 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_disparity_graph-agx_orin.json)
14 ms | [75.1 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_disparity_graph-orin_nx.json)
20 ms | [50.6 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_disparity_graph-orin_nano.json)
28 ms | [387 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_disparity_graph-nuc_4060ti.json)
8.3 ms | ## Table of Contents @@ -60,7 +60,7 @@ The following table summarizes the per-platform performance statistics of sample ## Latest Update -Update 2023-04-05: Source available GXF extensions +Update 2023-05-25: Improved stereo rectification. ## Supported Platforms @@ -248,6 +248,7 @@ admin@workstation:/workspaces/isaac_ros-dev$ ros2 launch realsense2_camera rs_l | Date | Changes | | ---------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------- | +| 2023-05-25 | Improved stereo rectification | | 2023-04-05 | Source available GXF extensions | | 2022-10-19 | Updated OSS licensing | | 2022-08-31 | Image flip support and update to be compatible with JetPack 5.0.2 | diff --git a/isaac_ros_image_pipeline/package.xml b/isaac_ros_image_pipeline/package.xml index 85e6ab8..972fd11 100644 --- a/isaac_ros_image_pipeline/package.xml +++ b/isaac_ros_image_pipeline/package.xml @@ -21,7 +21,7 @@ SPDX-License-Identifier: Apache-2.0 isaac_ros_image_pipeline - 0.30.0 + 0.31.0 Core image processing Hemal Shah diff --git a/isaac_ros_image_proc/CMakeLists.txt b/isaac_ros_image_proc/CMakeLists.txt index 9df7655..a33c78d 100644 --- a/isaac_ros_image_proc/CMakeLists.txt +++ b/isaac_ros_image_proc/CMakeLists.txt @@ -75,6 +75,10 @@ install(TARGETS gxf_image_flip DESTINATION share/${PROJECT_NAME}/gxf/lib/image_p add_subdirectory(gxf/tensorops) install(TARGETS gxf_tensorops DESTINATION share/${PROJECT_NAME}/gxf/lib/image_proc) +# RectifyParamsGenerator +add_subdirectory(gxf/rectify_params_generator) +install(TARGETS gxf_rectify_params_generator DESTINATION share/${PROJECT_NAME}/gxf/lib/image_proc) + ### End extensions if(BUILD_TESTING) @@ -88,12 +92,14 @@ if(BUILD_TESTING) add_launch_test(test/isaac_ros_image_format_converter_grayscale_test.py) add_launch_test(test/isaac_ros_image_format_converter_nv24_test.py) add_launch_test(test/isaac_ros_rectify_test.py) + add_launch_test(test/isaac_ros_rectify_timestamp_match_test.py) + add_launch_test(test/isaac_ros_rectify_oss_comparison_test.py) + add_launch_test(test/isaac_ros_rectify_stereo_epipolar_test.py) add_launch_test(test/isaac_ros_resize_test.py) add_launch_test(test/isaac_ros_resize_invalid_test.py) add_launch_test(test/isaac_ros_image_flip_180_test.py) add_launch_test(test/isaac_ros_image_flip_hori_test.py) add_launch_test(test/isaac_ros_image_flip_vert_test.py) - endif() ament_auto_package(INSTALL_TO_SHARE launch config) diff --git a/isaac_ros_image_proc/config/image_format_converter_substitution.yaml b/isaac_ros_image_proc/config/image_format_converter_substitution.yaml index cb9fad0..d20efe6 100644 --- a/isaac_ros_image_proc/config/image_format_converter_substitution.yaml +++ b/isaac_ros_image_proc/config/image_format_converter_substitution.yaml @@ -22,7 +22,7 @@ body: groups: - entities: - components: - - type: nvidia::cvcore::tensor_ops::ConvertColorFormat + - type: nvidia::isaac::tensor_ops::ConvertColorFormat parameters: output_type: "BGR_U8" receiver: data_receiver @@ -34,7 +34,7 @@ body: overriding_parameters: [] - entities: - components: - - type: nvidia::cvcore::tensor_ops::StreamConvertColorFormat + - type: nvidia::isaac::tensor_ops::StreamConvertColorFormat parameters: output_type: "NV24" receiver: data_receiver diff --git a/isaac_ros_image_proc/config/namespace_injector_rule_image_format_converter.yaml b/isaac_ros_image_proc/config/namespace_injector_rule_image_format_converter.yaml index c5fb076..38b48a8 100644 --- a/isaac_ros_image_proc/config/namespace_injector_rule_image_format_converter.yaml +++ b/isaac_ros_image_proc/config/namespace_injector_rule_image_format_converter.yaml @@ -20,7 +20,7 @@ name: Image Format Converter Namespace Injector Rule operation: namespace_injector body: components: - - type: nvidia::cvcore::tensor_ops::ConvertColorFormat + - type: nvidia::isaac::tensor_ops::ConvertColorFormat path_parameter_keys: [input_adapter, output_adapter] - - type: nvidia::cvcore::tensor_ops::StreamConvertColorFormat + - type: nvidia::isaac::tensor_ops::StreamConvertColorFormat path_parameter_keys: [input_adapter, output_adapter, stream] diff --git a/isaac_ros_image_proc/config/namespace_injector_rule_rectify.yaml b/isaac_ros_image_proc/config/namespace_injector_rule_rectify.yaml index 80d38ac..656b9b2 100644 --- a/isaac_ros_image_proc/config/namespace_injector_rule_rectify.yaml +++ b/isaac_ros_image_proc/config/namespace_injector_rule_rectify.yaml @@ -20,5 +20,5 @@ name: Image Rectify Namespace Injector Rule operation: namespace_injector body: components: - - type: nvidia::cvcore::tensor_ops::StreamUndistort + - type: nvidia::isaac::tensor_ops::StreamUndistort path_parameter_keys: [input_adapter, output_adapter, stream] diff --git a/isaac_ros_image_proc/config/namespace_injector_rule_resize.yaml b/isaac_ros_image_proc/config/namespace_injector_rule_resize.yaml index b692a89..2288d76 100644 --- a/isaac_ros_image_proc/config/namespace_injector_rule_resize.yaml +++ b/isaac_ros_image_proc/config/namespace_injector_rule_resize.yaml @@ -20,5 +20,5 @@ name: Image Resize Namespace Injector Rule operation: namespace_injector body: components: - - type: nvidia::cvcore::tensor_ops::Resize + - type: nvidia::isaac::tensor_ops::Resize path_parameter_keys: [input_adapter, output_adapter] diff --git a/isaac_ros_image_proc/config/nitros_image_flip_node.yaml b/isaac_ros_image_proc/config/nitros_image_flip_node.yaml index f42966c..36e6148 100644 --- a/isaac_ros_image_proc/config/nitros_image_flip_node.yaml +++ b/isaac_ros_image_proc/config/nitros_image_flip_node.yaml @@ -1,6 +1,6 @@ %YAML 1.2 # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# Copyright (c) 2022-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -53,7 +53,7 @@ components: backends: "CUDA" mode: "BOTH" --- -name: vault +name: sink components: - name: signal type: nvidia::gxf::DoubleBufferReceiver @@ -64,24 +64,27 @@ components: parameters: receiver: signal min_size: 1 -- name: vault - type: nvidia::gxf::Vault +- name: sink + type: nvidia::isaac_ros::MessageRelay parameters: source: signal - max_waiting_count: 1 - drop_waiting: false --- components: - type: nvidia::gxf::Connection parameters: source: image_flip/data_transmitter - target: vault/signal + target: sink/signal --- name: utils components: - name: clock type: nvidia::gxf::RealtimeClock -- type: nvidia::gxf::GreedyScheduler +- type: nvidia::gxf::MultiThreadScheduler parameters: clock: clock stop_on_deadlock: false + check_recession_period_ms: 1 + worker_thread_number: 2 +- type: nvidia::gxf::JobStatistics + parameters: + clock: clock diff --git a/isaac_ros_image_proc/config/nitros_image_format_converter_node.yaml b/isaac_ros_image_proc/config/nitros_image_format_converter_node.yaml index cc9a331..fab5f37 100644 --- a/isaac_ros_image_proc/config/nitros_image_format_converter_node.yaml +++ b/isaac_ros_image_proc/config/nitros_image_format_converter_node.yaml @@ -1,6 +1,6 @@ %YAML 1.2 # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# Copyright (c) 2022-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -43,7 +43,7 @@ components: block_size: 14400000 num_blocks: 40 - name: converter - type: nvidia::cvcore::tensor_ops::ConvertColorFormat + type: nvidia::isaac::tensor_ops::ConvertColorFormat parameters: output_type: "RGB_U8" receiver: data_receiver @@ -53,7 +53,7 @@ components: output_adapter: resource/adapter output_name: "image" --- -name: vault +name: sink components: - name: signal type: nvidia::gxf::DoubleBufferReceiver @@ -64,21 +64,19 @@ components: parameters: receiver: signal min_size: 1 -- name: vault - type: nvidia::gxf::Vault +- name: sink + type: nvidia::isaac_ros::MessageRelay parameters: source: signal - max_waiting_count: 1 - drop_waiting: false --- name: resource components: - name: adapter - type: nvidia::cvcore::tensor_ops::ImageAdapter + type: nvidia::isaac::tensor_ops::ImageAdapter parameters: message_type: "VideoBuffer" - name: stream - type: nvidia::cvcore::tensor_ops::TensorStream + type: nvidia::isaac::tensor_ops::TensorStream parameters: backend_type: "VPI" engine_type: "GPU" @@ -87,7 +85,7 @@ components: - type: nvidia::gxf::Connection parameters: source: imageConverter/data_transmitter - target: vault/signal + target: sink/signal --- name: utils components: @@ -97,3 +95,7 @@ components: parameters: clock: clock stop_on_deadlock: false + check_recession_period_us: 100 +- type: nvidia::gxf::JobStatistics + parameters: + clock: clock diff --git a/isaac_ros_image_proc/config/nitros_rectify_node.yaml b/isaac_ros_image_proc/config/nitros_rectify_node.yaml index 28f9b2c..9b5d4f9 100644 --- a/isaac_ros_image_proc/config/nitros_rectify_node.yaml +++ b/isaac_ros_image_proc/config/nitros_rectify_node.yaml @@ -1,6 +1,6 @@ %YAML 1.2 # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# Copyright (c) 2022-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -73,7 +73,7 @@ components: transmitter: data_transmitter min_size: 1 - name: input_camera - type: nvidia::cvcore::tensor_ops::CameraModel + type: nvidia::isaac::tensor_ops::CameraModel parameters: distortion_type: "FisheyeEquidistant" distortion_coefficients: [0, 0, 0, 0, @@ -82,7 +82,7 @@ components: principle_point: [0, 0] skew_value: 0.0 - name: output_camera - type: nvidia::cvcore::tensor_ops::CameraModel + type: nvidia::isaac::tensor_ops::CameraModel parameters: distortion_type: "FisheyeEquidistant" distortion_coefficients: [0, 0, 0, 0, @@ -91,7 +91,7 @@ components: principle_point: [0, 0] skew_value: 0.0 - name: reference_frame - type: nvidia::cvcore::tensor_ops::Frame3D + type: nvidia::isaac::tensor_ops::Frame3D parameters: rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1] translation: [0, 0, 0] @@ -102,7 +102,7 @@ components: block_size: 7372800 num_blocks: 40 - name: undistort_algo - type: nvidia::cvcore::tensor_ops::StreamUndistort + type: nvidia::isaac::tensor_ops::StreamUndistort parameters: input_camera_model: input_camera reference_frame: reference_frame @@ -126,7 +126,7 @@ components: - name: data_receiver type: nvidia::gxf::DoubleBufferReceiver parameters: - capacity: 1 + capacity: 2 policy: 0 - type: nvidia::gxf::MessageAvailableSchedulingTerm parameters: @@ -157,7 +157,7 @@ components: image_tx: image_transmitter camera_model_tx: camera_model_transmitter --- -name: image_vault +name: image_sink components: - name: signal type: nvidia::gxf::DoubleBufferReceiver @@ -168,14 +168,12 @@ components: parameters: receiver: signal min_size: 1 -- name: vault - type: nvidia::gxf::Vault +- name: sink + type: nvidia::isaac_ros::MessageRelay parameters: source: signal - max_waiting_count: 1 - drop_waiting: false --- -name: camerainfo_vault +name: camerainfo_sink components: - name: signal type: nvidia::gxf::DoubleBufferReceiver @@ -186,22 +184,20 @@ components: parameters: receiver: signal min_size: 1 -- name: vault - type: nvidia::gxf::Vault +- name: sink + type: nvidia::isaac_ros::MessageRelay parameters: source: signal - max_waiting_count: 1 - drop_waiting: false --- name: resource components: - name: stream - type: nvidia::cvcore::tensor_ops::TensorStream + type: nvidia::isaac::tensor_ops::TensorStream parameters: backend_type: "VPI" engine_type: "GPU" - name: adapter - type: nvidia::cvcore::tensor_ops::ImageAdapter + type: nvidia::isaac::tensor_ops::ImageAdapter parameters: message_type: "VideoBuffer" --- @@ -217,17 +213,22 @@ components: - type: nvidia::gxf::Connection parameters: source: splitter/image_transmitter - target: image_vault/signal + target: image_sink/signal - type: nvidia::gxf::Connection parameters: source: splitter/camera_model_transmitter - target: camerainfo_vault/signal + target: camerainfo_sink/signal --- name: utils components: - name: clock type: nvidia::gxf::RealtimeClock -- type: nvidia::gxf::GreedyScheduler +- type: nvidia::gxf::MultiThreadScheduler parameters: clock: clock stop_on_deadlock: false + check_recession_period_ms: 1 + worker_thread_number: 2 +- type: nvidia::gxf::JobStatistics + parameters: + clock: clock diff --git a/isaac_ros_image_proc/config/nitros_resize_node.yaml b/isaac_ros_image_proc/config/nitros_resize_node.yaml index a523e6a..7d2e67e 100644 --- a/isaac_ros_image_proc/config/nitros_resize_node.yaml +++ b/isaac_ros_image_proc/config/nitros_resize_node.yaml @@ -1,6 +1,6 @@ %YAML 1.2 # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# Copyright (c) 2022-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -79,7 +79,7 @@ components: block_size: 7372800 num_blocks: 40 - name: StreamResize - type: nvidia::cvcore::tensor_ops::Resize + type: nvidia::isaac::tensor_ops::Resize parameters: output_width: 400 output_height: 300 @@ -98,7 +98,7 @@ components: - name: data_receiver type: nvidia::gxf::DoubleBufferReceiver parameters: - capacity: 1 + capacity: 2 policy: 0 - type: nvidia::gxf::MessageAvailableSchedulingTerm parameters: @@ -129,7 +129,7 @@ components: image_tx: image_transmitter camera_model_tx: camera_model_transmitter --- -name: image_vault +name: image_sink components: - name: signal type: nvidia::gxf::DoubleBufferReceiver @@ -140,14 +140,12 @@ components: parameters: receiver: signal min_size: 1 -- name: vault - type: nvidia::gxf::Vault +- name: sink + type: nvidia::isaac_ros::MessageRelay parameters: source: signal - max_waiting_count: 1 - drop_waiting: false --- -name: camerainfo_vault +name: camerainfo_sink components: - name: signal type: nvidia::gxf::DoubleBufferReceiver @@ -158,21 +156,19 @@ components: parameters: receiver: signal min_size: 1 -- name: vault - type: nvidia::gxf::Vault +- name: sink + type: nvidia::isaac_ros::MessageRelay parameters: source: signal - max_waiting_count: 1 - drop_waiting: false --- name: resource components: - name: adapter - type: nvidia::cvcore::tensor_ops::ImageAdapter + type: nvidia::isaac::tensor_ops::ImageAdapter parameters: message_type: "VideoBuffer" - name: stream - type: nvidia::cvcore::tensor_ops::TensorStream + type: nvidia::isaac::tensor_ops::TensorStream parameters: backend_type: "VPI" engine_type: "CPU" @@ -189,17 +185,22 @@ components: - type: nvidia::gxf::Connection parameters: source: splitter/image_transmitter - target: image_vault/signal + target: image_sink/signal - type: nvidia::gxf::Connection parameters: source: splitter/camera_model_transmitter - target: camerainfo_vault/signal + target: camerainfo_sink/signal --- name: utils components: - name: clock type: nvidia::gxf::RealtimeClock -- type: nvidia::gxf::GreedyScheduler +- type: nvidia::gxf::MultiThreadScheduler parameters: clock: clock stop_on_deadlock: false + check_recession_period_ms: 1 + worker_thread_number: 2 +- type: nvidia::gxf::JobStatistics + parameters: + clock: clock diff --git a/isaac_ros_image_proc/gxf/rectify_params_generator/CMakeLists.txt b/isaac_ros_image_proc/gxf/rectify_params_generator/CMakeLists.txt new file mode 100644 index 0000000..b6ab1cb --- /dev/null +++ b/isaac_ros_image_proc/gxf/rectify_params_generator/CMakeLists.txt @@ -0,0 +1,70 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +project(gxf_rectify_params_generator LANGUAGES C CXX) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-fPIC -w) +endif() + +# Dependencies +include(YamlCpp) +find_package(GXF ${ISAAC_ROS_GXF_VERSION} MODULE REQUIRED + multimedia + isaac_messages +) +find_package(OpenCV REQUIRED) +find_package(Eigen3 3.3 REQUIRED NO_MODULE) + +# Create extension +add_library(gxf_rectify_params_generator SHARED + extensions/rectify/rectify.cpp + extensions/rectify/components/rectify_params_generator.cpp + extensions/rectify/components/rectify_params_generator.hpp + extensions/rectify/components/stereo_extrinsics_normalizer.cpp + extensions/rectify/components/stereo_extrinsics_normalizer.hpp +) +target_include_directories(gxf_rectify_params_generator PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) + + +add_library(gxf_rectify_utils STATIC +extensions/rectify/utils/camera_utils.cpp +extensions/rectify/utils/camera_utils.hpp +extensions/rectify/utils/utils.cpp +extensions/rectify/utils/utils.hpp +) +target_include_directories(gxf_rectify_utils PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) + +target_link_libraries(gxf_rectify_utils + PUBLIC + yaml-cpp + PRIVATE + Eigen3::Eigen + GXF::multimedia + GXF::isaac_messages + ${OpenCV_LIBS} +) + +target_link_libraries(gxf_rectify_params_generator + PUBLIC + GXF::multimedia + GXF::isaac_messages + ${OpenCV_LIBS} + PRIVATE + gxf_rectify_utils + Eigen3::Eigen +) diff --git a/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/components/rectify_params_generator.cpp b/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/components/rectify_params_generator.cpp new file mode 100644 index 0000000..8cf2046 --- /dev/null +++ b/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/components/rectify_params_generator.cpp @@ -0,0 +1,342 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 +#include "extensions/rectify/components/rectify_params_generator.hpp" + +#include + +#include +#include +#include + +#include "extensions/messages/camera_message.hpp" +#include "extensions/rectify/utils/camera_utils.hpp" +#include "gems/gxf_helpers/expected_macro.hpp" +#include "gxf/multimedia/video.hpp" +#include "gxf/std/timestamp.hpp" + +namespace nvidia { +namespace isaac { +namespace { + +void PrintRectifyParameters(cv::Matx33d R1, cv::Matx33d R2, + cv::Matx34d P1, cv::Matx34d P2, cv::Matx44d Q) { + GXF_LOG_DEBUG("left camera Rotation: \n[%g, %g, %g, \n%g, %g, %g, \n%g, %g, %g]\n", + R1(0, 0), R1(0, 1), R1(0, 2), R1(0, 3), R1(0, 4), R1(0, 5), R1(0, 6), R1(0, 7), R1(0, 8)); + + GXF_LOG_DEBUG("right camera Rotation: \n[%g, %g, %g, \n%g, %g, %g, \n%g, %g, %g]\n", + R2(0, 0), R2(0, 1), R2(0, 2), R2(0, 3), R2(0, 4), R2(0, 5), R2(0, 6), R2(0, 7), R2(0, 8)); + + GXF_LOG_DEBUG("left camera projection: \n[%g, %g, %g, %g, \n%g, %g, %g, %g," + "\n%g, %g, %g, %g]\n", + P1(0, 0), P1(0, 1), P1(0, 2), P1(0, 3), + P1(0, 4), P1(0, 5), P1(0, 6), P1(0, 7), + P1(0, 8), P1(0, 9), P1(0, 10), P1(0, 11)); + + GXF_LOG_DEBUG("right camera projection: \n[%g, %g, %g, %g, \n%g, %g, %g, %g," + "\n%g, %g, %g, %g]\n", + P2(0, 0), P2(0, 1), P2(0, 2), P2(0, 3), + P2(0, 4), P2(0, 5), P2(0, 6), P2(0, 7), + P2(0, 8), P2(0, 9), P2(0, 10), P2(0, 11)); + + GXF_LOG_DEBUG("diaparity-to-depth: \n[%g, %g, %g, %g, \n%g, %g, %g, %g," + "\n%g, %g, %g, %g, \n%g, %g, %g, %g]\n", + Q(0, 0), Q(0, 1), Q(0, 2), Q(0, 3), + Q(0, 4), Q(0, 5), Q(0, 6), Q(0, 7), + Q(0, 8), Q(0, 9), Q(0, 10), Q(0, 11), + Q(0, 12), Q(0, 13), Q(0, 14), Q(0, 15)); +} + +void InitCameraMatrixFromModel( + const gxf::CameraModel& intrinsics, + cv::Matx33d& camera_matrix) { + camera_matrix = {intrinsics.focal_length.x, 0.0, intrinsics.principal_point.x, \ + 0.0, intrinsics.focal_length.y, intrinsics.principal_point.y, \ + 0.0, 0.0, 1.0}; +} + +void InitDistortionCoefficients( + const gxf::CameraModel& intrinsics, + cv::Matx& coefficents) { + coefficents = { + intrinsics.distortion_coefficients[0], + intrinsics.distortion_coefficients[1], + intrinsics.distortion_coefficients[6], + intrinsics.distortion_coefficients[7], + intrinsics.distortion_coefficients[2], + intrinsics.distortion_coefficients[3], + intrinsics.distortion_coefficients[4], + intrinsics.distortion_coefficients[5] + }; +} + +gxf::Expected CopyVideoBuffer( + gxf::Handle input, + gxf::Handle output) { + if (!input || !output) { + return gxf::Unexpected{GXF_ARGUMENT_NULL}; + } + // Copy image frame using same storage type + cudaMemcpyKind operation; + switch (input->storage_type()) { + case gxf::MemoryStorageType::kHost: + case gxf::MemoryStorageType::kSystem: { + operation = cudaMemcpyHostToHost; + } break; + case gxf::MemoryStorageType::kDevice: { + operation = cudaMemcpyDeviceToDevice; + } break; + default: + return gxf::Unexpected{GXF_MEMORY_INVALID_STORAGE_MODE}; + } + + // TODO(tazhang) zero-copy forwarding when GXF implements GXF-490 + const cudaError_t error = cudaMemcpy( + output->pointer(), + input->pointer(), + input->size(), + operation); + if (error != cudaSuccess) { + GXF_LOG_ERROR("%s", cudaGetErrorString(error)); + return gxf::Unexpected{GXF_FAILURE}; + } + + return gxf::Success; +} + +} // namespace + +RectifyParamsGenerator::RectifyParamsGenerator() {} + +RectifyParamsGenerator::~RectifyParamsGenerator() {} + +gxf_result_t RectifyParamsGenerator::registerInterface(gxf::Registrar* registrar) { + gxf::Expected result; + result &= registrar->parameter( + left_camera_input_, "left_camera_input", "Left Camera Model input", + "Camera model input for left camera"); + result &= registrar->parameter( + left_camera_output_, "left_camera_output", "Left Camera Model output", + "Camera model output for left camera rectification"); + result &= registrar->parameter( + right_camera_input_, "right_camera_input", "Right Camera Model input", + "Camera model input for left camera"); + result &= registrar->parameter( + right_camera_output_, "right_camera_output", "Right Camera Model output", + "Camera model output for right camera rectification"); + result &= registrar->parameter( + allocator_, "allocator", "Allocator", + "Memory allocator"); + result &= registrar->parameter( + alpha_, "alpha", "Alpha", + "Free scaling parameter between 0 and 1", 0.0); + result &= registrar->parameter( + left_camera_model_file_, "left_camera_model_file", "File name for left camera model", + "File that contains paramemters that defines a camera model", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + result &= registrar->parameter( + right_camera_model_file_, "right_camera_model_file", "File name for right camera model", + "File that contains paramemters that defines a camera model", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + return gxf::ToResultCode(result); +} + +gxf_result_t RectifyParamsGenerator::start() { + use_camera_model_file_ = false; + std::string filename[2]; + filename[0] = left_camera_model_file_.try_get().value_or(""); + if (filename[0].empty()) { + return GXF_SUCCESS; + } + filename[1] = right_camera_model_file_.try_get().value_or(""); + if (filename[1].empty()) { + return GXF_SUCCESS; + } + + for (auto idx = 0; idx < 2; idx++) { + const auto maybe_json = ::isaac::serialization::TryLoadJsonFromFile(filename[idx]); + if (!maybe_json) { + GXF_LOG_ERROR("Failed to read json from file '%s'", filename[idx].c_str()); + return GXF_ARGUMENT_NULL; + } + const nlohmann::json& json = *maybe_json; + if (!cameraUtils::importCameraIntrinsics(json["model"], camera_intrinsics_[idx])) { + GXF_LOG_ERROR("Can't read model from json"); + return GXF_ARGUMENT_NULL; + } + camera_intrinsics_[idx].distortion_type = gxf::DistortionType::Polynomial; + if (!cameraUtils::importCameraExtrinsics(json["model"], camera_extrinsics_[idx], true)) { + GXF_LOG_ERROR("Can't read camera extrinsics from json"); + return GXF_ARGUMENT_NULL; + } + cameraUtils::printCameraInfo(camera_intrinsics_[idx], camera_extrinsics_[idx]); + } + + use_camera_model_file_ = true; + return GXF_SUCCESS; +} + +gxf_result_t RectifyParamsGenerator::tick() { + // Receive messages + CameraMessageParts left_input; + CameraMessageParts right_input; + + if (use_camera_model_file_) { + GXF_RETURN_IF_ERROR(left_camera_input_->receive().assign_to(left_input.entity)); + left_input.frame = GXF_UNWRAP_OR_RETURN(left_input.entity.get()); + left_input.sequence_number = + GXF_UNWRAP_OR_RETURN(left_input.entity.get("sequence_number")); + left_input.timestamp = GXF_UNWRAP_OR_RETURN(left_input.entity.get()); + + GXF_RETURN_IF_ERROR(right_camera_input_->receive().assign_to(right_input.entity)); + right_input.frame = GXF_UNWRAP_OR_RETURN(right_input.entity.get()); + right_input.sequence_number = + GXF_UNWRAP_OR_RETURN(right_input.entity.get("sequence_number")); + right_input.timestamp = GXF_UNWRAP_OR_RETURN(right_input.entity.get()); + + GXF_RETURN_IF_ERROR(computeRectifyParams(camera_intrinsics_[0], camera_intrinsics_[1], + camera_extrinsics_[1])); + + } else { + left_input = GXF_UNWRAP_OR_RETURN(left_camera_input_->receive().map(GetCameraMessage)); + right_input = GXF_UNWRAP_OR_RETURN(right_camera_input_->receive().map(GetCameraMessage)); + GXF_RETURN_IF_ERROR(computeRectifyParams(*left_input.intrinsics, *right_input.intrinsics, + *right_input.extrinsics)); + } + + // Create left output message + CameraMessageParts left_output = GXF_UNWRAP_OR_RETURN(CreateCameraMessage( + context(), left_input.frame->video_frame_info(), left_input.frame->size(), + left_input.frame->storage_type(), allocator_)); + + gxf::Handle left_target_camera = + GXF_UNWRAP_OR_RETURN(left_output.entity.add("target_camera")); + gxf::Handle left_target_extrinsics = + GXF_UNWRAP_OR_RETURN(left_output.entity.add("target_extrinsics_delta")); + // We put rectified_camera_rotation_raw_camera here + *left_target_extrinsics = rectify_extrinsics_[0]; + GXF_RETURN_IF_ERROR(CopyVideoBuffer(left_input.frame, left_output.frame)); + *left_target_camera = rectify_intrinsics_[0]; + *left_output.intrinsics = use_camera_model_file_ ? camera_intrinsics_[0] : *left_input.intrinsics; + // This puts stereo extrinsics in + *left_output.extrinsics = use_camera_model_file_ ? camera_extrinsics_[0] : *left_input.extrinsics; + *left_output.sequence_number = *left_input.sequence_number; + *left_output.timestamp = *left_input.timestamp; + + // Create right output message + CameraMessageParts right_output = GXF_UNWRAP_OR_RETURN(CreateCameraMessage( + context(), right_input.frame->video_frame_info(), right_input.frame->size(), + right_input.frame->storage_type(), allocator_)); + // We put rectified_camera_rotation_raw_camera here + gxf::Handle right_target_extrinsics = + GXF_UNWRAP_OR_RETURN(right_output.entity.add("target_extrinsics_delta")); + *right_target_extrinsics = rectify_extrinsics_[1]; + gxf::Handle right_target_camera = + GXF_UNWRAP_OR_RETURN(right_output.entity.add("target_camera")); + GXF_RETURN_IF_ERROR(CopyVideoBuffer(right_input.frame, right_output.frame)); + *right_target_camera = rectify_intrinsics_[1]; + *right_output.intrinsics = use_camera_model_file_ ? camera_intrinsics_[1] : + *right_input.intrinsics; + *right_output.extrinsics = + use_camera_model_file_ ? camera_extrinsics_[1] : *right_input.extrinsics; + *right_output.sequence_number = *right_input.sequence_number; + *right_output.timestamp = *right_input.timestamp; + + GXF_RETURN_IF_ERROR(left_camera_output_->publish(left_output.entity, + left_input.timestamp->acqtime)); + GXF_RETURN_IF_ERROR(right_camera_output_->publish(right_output.entity, + right_input.timestamp->acqtime)); + + return GXF_SUCCESS; +} + +gxf::Expected RectifyParamsGenerator::computeRectifyParams( + const gxf::CameraModel& left_camera_intrinsics, + const gxf::CameraModel& right_camera_intrinsics, + const gxf::Pose3D& right_extrinsics) { + cv::Size imagesize(left_camera_intrinsics.dimensions.x, + left_camera_intrinsics.dimensions.y); + + cv::Matx33d left_intrinsics; + InitCameraMatrixFromModel(left_camera_intrinsics, left_intrinsics); + cv::Matx left_coefficients; + InitDistortionCoefficients(left_camera_intrinsics, left_coefficients); + + cv::Matx33d right_intrinsics; + InitCameraMatrixFromModel(right_camera_intrinsics, right_intrinsics); + cv::Matx right_coefficients; + InitDistortionCoefficients(right_camera_intrinsics, right_coefficients); + + cv::Matx31d Tlr = { right_extrinsics.translation[0], + right_extrinsics.translation[1], + right_extrinsics.translation[2]}; + + cv::Matx33d Rlr = { right_extrinsics.rotation[0], + right_extrinsics.rotation[1], + right_extrinsics.rotation[2], + right_extrinsics.rotation[3], + right_extrinsics.rotation[4], + right_extrinsics.rotation[5], + right_extrinsics.rotation[6], + right_extrinsics.rotation[7], + right_extrinsics.rotation[8]}; + + cv::Matx33d R[2]; + cv::Matx34d P[2]; + cv::Matx44d Q; + cv::stereoRectify( + left_intrinsics, left_coefficients, + right_intrinsics, right_coefficients, + imagesize, Rlr, Tlr, + R[0], R[1], + P[0], P[1], + Q, cv::CALIB_ZERO_DISPARITY, alpha_); + + PrintRectifyParameters(R[0], R[1], P[0], P[1], Q); + + for (int i = 0; i < 2; i++) { + rectify_intrinsics_[i].dimensions = left_camera_intrinsics.dimensions; + + rectify_intrinsics_[i].distortion_type = gxf::DistortionType::Perspective; + memset(rectify_intrinsics_[i].distortion_coefficients, 0, + rectify_intrinsics_[i].kMaxDistortionCoefficients * sizeof(float)); + + rectify_intrinsics_[i].focal_length.x = P[i](0, 0); + rectify_intrinsics_[i].focal_length.y = P[i](0, 5); + rectify_intrinsics_[i].principal_point.x = P[i](0, 2); + rectify_intrinsics_[i].principal_point.y = P[i](0, 6); + rectify_intrinsics_[i].skew_value = 0; + + rectify_extrinsics_[i].translation = {0, 0, 0}; + rectify_extrinsics_[i].rotation = { + static_cast(R[i](0, 0)), + static_cast(R[i](0, 1)), + static_cast(R[i](0, 2)), + static_cast(R[i](0, 3)), + static_cast(R[i](0, 4)), + static_cast(R[i](0, 5)), + static_cast(R[i](0, 6)), + static_cast(R[i](0, 7)), + static_cast(R[i](0, 8))}; + + cameraUtils::printCameraInfo(rectify_intrinsics_[i], rectify_extrinsics_[i]); + } + + return gxf::Success; +} + +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/components/rectify_params_generator.hpp b/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/components/rectify_params_generator.hpp new file mode 100644 index 0000000..e0cfb29 --- /dev/null +++ b/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/components/rectify_params_generator.hpp @@ -0,0 +1,71 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 +#pragma once + +#include +#include +#include + +#include "gxf/multimedia/camera.hpp" +#include "gxf/std/allocator.hpp" +#include "gxf/std/codelet.hpp" +#include "gxf/std/receiver.hpp" +#include "gxf/std/transmitter.hpp" + +namespace nvidia { +namespace isaac { + +// Takes an camera model as input and generate rectify parameters. +class RectifyParamsGenerator : public gxf::Codelet { + public: + // Explicitly declare constructors and destructors + // to get around forward declaration of AprilTagsData + RectifyParamsGenerator(); + ~RectifyParamsGenerator(); + + gxf_result_t registerInterface(gxf::Registrar* registrar) override; + gxf_result_t initialize() { return GXF_SUCCESS; } + gxf_result_t deinitialize() { return GXF_SUCCESS; } + + gxf_result_t start() override; + gxf_result_t tick() override; + gxf_result_t stop() override { return GXF_SUCCESS; } + + private: + gxf::Expected computeRectifyParams( + const gxf::CameraModel& left_camera_intrinsics, + const gxf::CameraModel& right_camera_intrinsics, + const gxf::Pose3D& right_extrinsics); + + gxf::Parameter> left_camera_input_; + gxf::Parameter> right_camera_input_; + gxf::Parameter> left_camera_output_; + gxf::Parameter> right_camera_output_; + gxf::Parameter> allocator_; + gxf::Parameter left_camera_model_file_; + gxf::Parameter right_camera_model_file_; + gxf::Parameter alpha_; + + gxf::CameraModel rectify_intrinsics_[2]; + gxf::Pose3D rectify_extrinsics_[2]; + gxf::CameraModel camera_intrinsics_[2]; + gxf::Pose3D camera_extrinsics_[2]; + bool use_camera_model_file_; +}; + +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/components/stereo_extrinsics_normalizer.cpp b/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/components/stereo_extrinsics_normalizer.cpp new file mode 100644 index 0000000..4ffe004 --- /dev/null +++ b/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/components/stereo_extrinsics_normalizer.cpp @@ -0,0 +1,95 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 +#include "extensions/rectify/components/stereo_extrinsics_normalizer.hpp" + +#include "extensions/rectify/utils/utils.hpp" +#include "gems/gxf_helpers/expected_macro.hpp" +#include "gxf/multimedia/camera.hpp" + +namespace nvidia { +namespace isaac { + +gxf_result_t StereoExtrinsicsNormalizer::registerInterface(gxf::Registrar* registrar) { + gxf::Expected result; + result &= registrar->parameter( + left_camera_input_, "left_camera_input", "Left camera input", + "Left camera input containing extrinsics from StreamUndistort module"); + result &= registrar->parameter( + right_camera_input_, "right_camera_input", "Right camera input", + "Right camera input containing extrinsics from StreamUndistort module"); + result &= registrar->parameter( + left_camera_output_, "left_camera_output", "Left camera output", + "Left camera output containing extrinsics from StreamUndistort module"); + result &= registrar->parameter( + right_camera_output_, "right_camera_output", "Right camera output", + "Right camera output containing extrinsics from StreamUndistort module"); + return gxf::ToResultCode(result); +} + +gxf::Expected StereoExtrinsicsNormalizer::postProcessTransformation( + gxf::Entity& input_left_message, gxf::Entity& input_right_message) { + // Modify extrinsics information for left camera + gxf::Handle left_camera_extrinsics = + GXF_UNWRAP_OR_RETURN(input_left_message.get("extrinsics")); + // Modify extrinsics information for right camera + gxf::Handle right_camera_extrinsics = + GXF_UNWRAP_OR_RETURN(input_right_message.get("extrinsics")); + // Modify target extrinsics information for left camera + gxf::Handle left_camera_target_extrinsics = + GXF_UNWRAP_OR_RETURN(input_left_message.get("target_extrinsics_delta")); + // Modify target extrinsics information for right camera + gxf::Handle right_camera_target_extrinsics = + GXF_UNWRAP_OR_RETURN(input_right_message.get("target_extrinsics_delta")); + + // Do post processing of extrinsic data + ::isaac::Pose3d left_T_origin = + ConvertToIsaacPose(*left_camera_extrinsics.get()); + ::isaac::Pose3d right_T_left = + ConvertToIsaacPose(*right_camera_extrinsics.get()); + ::isaac::Pose3d left_rectified_T_left = + ConvertToIsaacPose(*left_camera_target_extrinsics.get()); + ::isaac::Pose3d right_rectified_T_right = + ConvertToIsaacPose(*right_camera_target_extrinsics.get()); + ::isaac::Pose3d rectified_right_T_rectified_left = + right_rectified_T_right * right_T_left * left_rectified_T_left.inverse(); + + gxf::Pose3D processed_right_extrinsics = ConvertToGxfPose( + rectified_right_T_rectified_left); + ::isaac::Pose3d origin_T_rectified_left = (left_rectified_T_left * left_T_origin).inverse(); + + gxf::Pose3D processed_left_extrinsics = ConvertToGxfPose( + origin_T_rectified_left); + + *left_camera_extrinsics.get() = processed_left_extrinsics; + *right_camera_extrinsics.get() = processed_right_extrinsics; + + // Publish messages to left camera output and right camera output + return left_camera_output_->publish(input_left_message).and_then([&] { + return right_camera_output_->publish(input_right_message); + }); +} + +gxf_result_t StereoExtrinsicsNormalizer::tick() { + // Receiving the left data + gxf::Entity input_left_message = GXF_UNWRAP_OR_RETURN(left_camera_input_->receive()); + // Receiving the right camera data + gxf::Entity input_right_message = GXF_UNWRAP_OR_RETURN(right_camera_input_->receive()); + return gxf::ToResultCode(postProcessTransformation(input_left_message, input_right_message)); +} + +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/components/stereo_extrinsics_normalizer.hpp b/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/components/stereo_extrinsics_normalizer.hpp new file mode 100644 index 0000000..b0445ce --- /dev/null +++ b/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/components/stereo_extrinsics_normalizer.hpp @@ -0,0 +1,57 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 +#pragma once + +#include +#include + +#include "gxf/std/codelet.hpp" +#include "gxf/std/receiver.hpp" +#include "gxf/std/transmitter.hpp" + +namespace nvidia { +namespace isaac { + +// Takes input messages from Undistort module and applies post processing to extrinsics messages. +// The post processing outputs a GXF message under "extrinsics" (gxf::Pose3D) for both left and +// right imagers such that: +// 1. rectified_right_T_rectified_left: Position of rectified left camera with respect to the +// rectified right camera +// 2. origin_T_rectified_left: Position of the rectified left camera with respect to +// the stereo camera origin +class StereoExtrinsicsNormalizer : public gxf::Codelet { + public: + gxf_result_t registerInterface(gxf::Registrar* registrar) override; + gxf_result_t initialize() { return GXF_SUCCESS; } + gxf_result_t deinitialize() { return GXF_SUCCESS; } + + gxf_result_t start() override { return GXF_SUCCESS; } + gxf_result_t tick() override; + gxf_result_t stop() override { return GXF_SUCCESS; } + + private: + gxf::Expected postProcessTransformation( + gxf::Entity& input_left_message, gxf::Entity& input_right_message); + + gxf::Parameter> left_camera_input_; + gxf::Parameter> right_camera_input_; + gxf::Parameter> left_camera_output_; + gxf::Parameter> right_camera_output_; +}; + +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/rectify.cpp b/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/rectify.cpp new file mode 100644 index 0000000..3d31e22 --- /dev/null +++ b/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/rectify.cpp @@ -0,0 +1,35 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2022-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 +#include "extensions/rectify/components/rectify_params_generator.hpp" +#include "extensions/rectify/components/stereo_extrinsics_normalizer.hpp" +#include "gxf/std/extension_factory_helper.hpp" + +GXF_EXT_FACTORY_BEGIN() + +GXF_EXT_FACTORY_SET_INFO(0xd8d7816ec0485ad4, 0xff795a414bd445ca, "RECTIFY", + "Extension containing components for rectify", + "Isaac SDK", "1.0.0", "LICENSE"); + +GXF_EXT_FACTORY_ADD(0xa9ddb12454d54aeb, 0x9739263d9fd1f635, + nvidia::isaac::RectifyParamsGenerator, nvidia::gxf::Codelet, + "Generate rectification parameters."); + +GXF_EXT_FACTORY_ADD(0xa9ddb12454d54aeb, 0x9738263d9fd1f635, + nvidia::isaac::StereoExtrinsicsNormalizer, nvidia::gxf::Codelet, + "Generate rectification undisortion post processing parameters."); + +GXF_EXT_FACTORY_END() diff --git a/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/utils/camera_utils.cpp b/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/utils/camera_utils.cpp new file mode 100644 index 0000000..0e86a84 --- /dev/null +++ b/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/utils/camera_utils.cpp @@ -0,0 +1,190 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 +#include "extensions/rectify/utils/camera_utils.hpp" + +#include +#include +#include + +#include "engine/gems/serialization/json.hpp" +#include "engine/gems/serialization/json_formatter.hpp" +#include "gxf/multimedia/camera.hpp" +#include "gxf/std/codelet.hpp" + +namespace nvidia { + +namespace isaac { + +namespace cameraUtils { +using Json = nlohmann::json; + +bool importCameraIntrinsics(const Json& json, gxf::CameraModel& intrinsics) { + try { + intrinsics.dimensions.x = json["dimensions"].at("width"); + intrinsics.dimensions.y = json["dimensions"].at("height"); + intrinsics.focal_length.x = json["focal_length"].at("fx"); + intrinsics.focal_length.y = json["focal_length"].at("fy"); + intrinsics.principal_point.x = json["principal_point"].at("cx"); + intrinsics.principal_point.y = json["principal_point"].at("cy"); + intrinsics.skew_value = json.at("skew"); + + std::string type = json.at("distortion_type"); + if (!type.compare("Polynomial")) + intrinsics.distortion_type = gxf::DistortionType::Polynomial; + else if (!type.compare("Perspective")) + intrinsics.distortion_type = gxf::DistortionType::Perspective; + else if (!type.compare("Equidistant")) + intrinsics.distortion_type = gxf::DistortionType::FisheyeEquidistant; + else if (!type.compare("Equisolid")) + intrinsics.distortion_type = gxf::DistortionType::FisheyeEquisolid; + else if (!type.compare("OrthoGraphic")) + intrinsics.distortion_type = gxf::DistortionType::FisheyeOrthoGraphic; + else if (!type.compare("Stereographic")) + intrinsics.distortion_type = gxf::DistortionType::FisheyeStereographic; + else if (!type.compare("Brown")) + intrinsics.distortion_type = gxf::DistortionType::Brown; + for (auto idx = 0; idx < intrinsics.kMaxDistortionCoefficients; idx++) { + intrinsics.distortion_coefficients[idx] = json.at("distortion_coefficients")[idx]; + } + } catch (Json::out_of_range& e) { + LOG_WARNING(e.what()); + return false; + } + return true; +} + +bool importCameraExtrinsics(const Json& json, + gxf::Pose3D& extrinsics, bool use_rodrigez) { + try { + for (auto idx = 0; idx < 3; idx++) { + extrinsics.translation[idx] = json.at("translation")[idx]; + } + if (use_rodrigez) { + cv::Matx33d Rlr; + cv::Matx31d aux(json.at("rotation_rodrigez")[0], + json.at("rotation_rodrigez")[1], + json.at("rotation_rodrigez")[2]); + cv::Rodrigues(aux, Rlr); + for (auto j = 0; j < 9; j++) { + extrinsics.rotation[j] = Rlr(j / 3, j % 3); + } + } else { + for (auto idx = 0; idx < 9; idx++) { + extrinsics.rotation[idx] = json.at("rotation")[idx]; + } + } + } catch (Json::out_of_range& e) { + LOG_WARNING(e.what()); + return false; + } + return true; +} + +void printCameraInfo(const gxf::CameraModel& intrinsics, + const gxf::Pose3D& extrinsics) { + GXF_LOG_DEBUG("intrinsics dimension: (%d, %d)", intrinsics.dimensions.x, + intrinsics.dimensions.y); + GXF_LOG_DEBUG("intrinsics principal_point: (%4.6f, %4.6f)", intrinsics.principal_point.x, + intrinsics.principal_point.y); + GXF_LOG_DEBUG("intrinsics focal_length: (%4.6f, %4.6f)", intrinsics.focal_length.x, + intrinsics.focal_length.y); + GXF_LOG_DEBUG("skew value: %g", intrinsics.skew_value); + GXF_LOG_DEBUG("distortion coeffs: [%g, %g, %g, %g, %g, %g, %g, %g]", + intrinsics.distortion_coefficients[0], + intrinsics.distortion_coefficients[1], + intrinsics.distortion_coefficients[2], + intrinsics.distortion_coefficients[3], + intrinsics.distortion_coefficients[4], + intrinsics.distortion_coefficients[5], + intrinsics.distortion_coefficients[6], + intrinsics.distortion_coefficients[7]); + + GXF_LOG_DEBUG("translation matrix:[%g, %g, %g]", + extrinsics.translation[0], + extrinsics.translation[1], + extrinsics.translation[2]); + GXF_LOG_DEBUG("rotation matrix:[%g, %g, %g, %g, %g, %g, %g, %g, %g]", + extrinsics.rotation[0], + extrinsics.rotation[1], + extrinsics.rotation[2], + extrinsics.rotation[3], + extrinsics.rotation[4], + extrinsics.rotation[5], + extrinsics.rotation[6], + extrinsics.rotation[7], + extrinsics.rotation[8]); +} + +void printCameraInfo(const gxf::CameraModel& source_intrinsics, + const gxf::CameraModel& target_intrinsics, + const gxf::Pose3D& extrinsics) { + GXF_LOG_DEBUG("=========SOURCE CAMERA==========="); + GXF_LOG_DEBUG("intrinsics dimension: (%d, %d)", source_intrinsics.dimensions.x, + source_intrinsics.dimensions.y); + GXF_LOG_DEBUG("intrinsics principal_point: (%4.6f, %4.6f)", source_intrinsics.principal_point.x, + source_intrinsics.principal_point.y); + GXF_LOG_DEBUG("intrinsics focal_length: (%4.6f, %4.6f)", source_intrinsics.focal_length.x, + source_intrinsics.focal_length.y); + GXF_LOG_DEBUG("skew value: %g", source_intrinsics.skew_value); + GXF_LOG_DEBUG("distortion coeffs: [%g, %g, %g, %g, %g, %g, %g, %g]", + source_intrinsics.distortion_coefficients[0], + source_intrinsics.distortion_coefficients[1], + source_intrinsics.distortion_coefficients[2], + source_intrinsics.distortion_coefficients[3], + source_intrinsics.distortion_coefficients[4], + source_intrinsics.distortion_coefficients[5], + source_intrinsics.distortion_coefficients[6], + source_intrinsics.distortion_coefficients[7]); + + GXF_LOG_DEBUG("=========TARGET CAMERA==========="); + GXF_LOG_DEBUG("intrinsics dimension: (%d, %d)", target_intrinsics.dimensions.x, + target_intrinsics.dimensions.y); + GXF_LOG_DEBUG("intrinsics principal_point: (%4.6f, %4.6f)", target_intrinsics.principal_point.x, + target_intrinsics.principal_point.y); + GXF_LOG_DEBUG("intrinsics focal_length: (%4.6f, %4.6f)", target_intrinsics.focal_length.x, + target_intrinsics.focal_length.y); + GXF_LOG_DEBUG("skew value: %g", target_intrinsics.skew_value); + GXF_LOG_DEBUG("distortion coeffs: [%g, %g, %g, %g, %g, %g, %g, %g]", + target_intrinsics.distortion_coefficients[0], + target_intrinsics.distortion_coefficients[1], + target_intrinsics.distortion_coefficients[2], + target_intrinsics.distortion_coefficients[3], + target_intrinsics.distortion_coefficients[4], + target_intrinsics.distortion_coefficients[5], + target_intrinsics.distortion_coefficients[6], + target_intrinsics.distortion_coefficients[7]); + + GXF_LOG_DEBUG("=========EXTRINSICS==========="); + GXF_LOG_DEBUG("translation matrix: [%g, %g, %g]", + extrinsics.translation[0], + extrinsics.translation[1], + extrinsics.translation[2]); + GXF_LOG_DEBUG("rotation matrix: [%g, %g, %g, %g, %g, %g, %g, %g, %g]", + extrinsics.rotation[0], + extrinsics.rotation[1], + extrinsics.rotation[2], + extrinsics.rotation[3], + extrinsics.rotation[4], + extrinsics.rotation[5], + extrinsics.rotation[6], + extrinsics.rotation[7], + extrinsics.rotation[8]); +} + +} // namespace cameraUtils +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/utils/camera_utils.hpp b/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/utils/camera_utils.hpp new file mode 100644 index 0000000..03f1b92 --- /dev/null +++ b/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/utils/camera_utils.hpp @@ -0,0 +1,36 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 +#pragma once + +#include "engine/gems/serialization/json.hpp" +#include "gxf/multimedia/camera.hpp" + +namespace nvidia { +namespace isaac { +namespace cameraUtils { +bool importCameraIntrinsics(const nlohmann::json& json, gxf::CameraModel& intrinsics); +bool importCameraExtrinsics(const nlohmann::json& json, gxf::Pose3D& extrinsics, + bool use_rodrigez); +void printCameraInfo(const gxf::CameraModel& intrinsics, + const gxf::Pose3D& extrinsics); +void printCameraInfo(const gxf::CameraModel& source_intrinsics, + const gxf::CameraModel& target_intrinsics, + const gxf::Pose3D& extrinsics); + +} // namespace cameraUtils +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/utils/utils.cpp b/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/utils/utils.cpp new file mode 100644 index 0000000..8dfffa3 --- /dev/null +++ b/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/utils/utils.cpp @@ -0,0 +1,54 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 +#include "extensions/rectify/utils/utils.hpp" + +#include "engine/core/math/pose3.hpp" +#include "gxf/multimedia/camera.hpp" +namespace nvidia { +namespace isaac { + +// TODO(kchahal) remove copy of ConvertToGxfPose and ConvertToIsaacPose in +// `calibration/file/utils.cpp` +gxf::Pose3D ConvertToGxfPose(const ::isaac::Pose3d& pose) { + gxf::Pose3D res; + auto matrix = pose.rotation.matrix(); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + res.rotation[(i * 3) + j] = matrix(i, j); + } + } + res.translation = { + static_cast(pose.translation.x()), static_cast(pose.translation.y()), + static_cast(pose.translation.z())}; + return res; +} + +::isaac::Pose3d ConvertToIsaacPose(const gxf::Pose3D& pose) { + ::isaac::Matrix3d matrix; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + matrix(i, j) = pose.rotation[(i * 3) + j]; + } + } + auto quaternion = ::isaac::SO3d::FromQuaternion(::isaac::Quaterniond(matrix)); + auto translation = + ::isaac::Vector3d(pose.translation[0], pose.translation[1], pose.translation[2]); + return ::isaac::Pose3d{quaternion, translation}; +} + +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/TensorList.h b/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/utils/utils.hpp similarity index 55% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/TensorList.h rename to isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/utils/utils.hpp index 4ec6695..30f9966 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/TensorList.h +++ b/isaac_ros_image_proc/gxf/rectify_params_generator/extensions/rectify/utils/utils.hpp @@ -1,5 +1,5 @@ // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2019-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -14,24 +14,19 @@ // limitations under the License. // // SPDX-License-Identifier: Apache-2.0 +#pragma once -#ifndef CVCORE_TENSORLIST_H -#define CVCORE_TENSORLIST_H +#include "engine/core/math/pose3.hpp" +#include "gxf/multimedia/camera.hpp" -#include +namespace nvidia { +namespace isaac { -#include "Traits.h" -#include "Array.h" -#include "Tensor.h" +// Converts an isaac pose object to a GXF pose object +gxf::Pose3D ConvertToGxfPose(const ::isaac::Pose3d& pose); -namespace cvcore { +// Converts GXFPose to Isaac pose +::isaac::Pose3d ConvertToIsaacPose(const gxf::Pose3D& pose); -/** - * @brief Implementation of a list of tensors of the same rank but, potentially, different dimensions - */ -template -using TensorList = typename std::enable_if::value, Array>::type; - -} // namespace cvcore - -#endif // CVCORE_TENSORLIST_H +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/CMakeLists.txt b/isaac_ros_image_proc/gxf/tensorops/CMakeLists.txt index 2792795..f07564b 100644 --- a/isaac_ros_image_proc/gxf/tensorops/CMakeLists.txt +++ b/isaac_ros_image_proc/gxf/tensorops/CMakeLists.txt @@ -25,105 +25,121 @@ endif() find_package(CUDAToolkit) find_package(vpi REQUIRED) find_package(GXF ${ISAAC_ROS_GXF_VERSION} MODULE REQUIRED + core cuda multimedia ) # Create extension add_library(gxf_tensorops SHARED - CameraModel.cpp - CameraModel.hpp - CMakeLists.txt - ConvertColorFormat.cpp - ConvertColorFormat.hpp - CropAndResize.cpp - CropAndResize.hpp - Frame3D.cpp - Frame3D.hpp - ImageAdapter.cpp - ImageAdapter.hpp - ImageUtils.cpp - ImageUtils.hpp - InterleavedToPlanar.cpp - InterleavedToPlanar.hpp - Normalize.cpp - Normalize.hpp - Reshape.cpp - Reshape.hpp - Resize.cpp - Resize.hpp - TensorOperator.cpp - TensorOperator.hpp - TensorOps.cpp - TensorStream.cpp - TensorStream.hpp - Undistort.cpp - Undistort.hpp - detail/ImageAdapterTensorImpl.cpp - detail/ImageAdapterTensorImpl.hpp - detail/ImageAdapterVideoBufferImpl.cpp - detail/ImageAdapterVideoBufferImpl.hpp + extensions/tensorops/tensorops.cpp + extensions/tensorops/components/CameraModel.cpp + extensions/tensorops/components/CameraModel.hpp + extensions/tensorops/components/ConvertColorFormat.cpp + extensions/tensorops/components/ConvertColorFormat.hpp + extensions/tensorops/components/CropAndResize.cpp + extensions/tensorops/components/CropAndResize.hpp + extensions/tensorops/components/Frame3D.cpp + extensions/tensorops/components/Frame3D.hpp + extensions/tensorops/components/ImageAdapter.cpp + extensions/tensorops/components/ImageAdapter.hpp + extensions/tensorops/components/ImageUtils.cpp + extensions/tensorops/components/ImageUtils.hpp + extensions/tensorops/components/InterleavedToPlanar.cpp + extensions/tensorops/components/InterleavedToPlanar.hpp + extensions/tensorops/components/Normalize.cpp + extensions/tensorops/components/Normalize.hpp + extensions/tensorops/components/Reshape.cpp + extensions/tensorops/components/Reshape.hpp + extensions/tensorops/components/Resize.cpp + extensions/tensorops/components/Resize.hpp + extensions/tensorops/components/TensorOperator.cpp + extensions/tensorops/components/TensorOperator.hpp + extensions/tensorops/components/TensorStream.cpp + extensions/tensorops/components/TensorStream.hpp + extensions/tensorops/components/Undistort.cpp + extensions/tensorops/components/Undistort.hpp + extensions/tensorops/components/detail/ImageAdapterTensorImpl.cpp + extensions/tensorops/components/detail/ImageAdapterTensorImpl.hpp + extensions/tensorops/components/detail/ImageAdapterVideoBufferImpl.cpp + extensions/tensorops/components/detail/ImageAdapterVideoBufferImpl.hpp ) -target_include_directories(gxf_tensorops PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/cvcore/include) +target_include_directories(gxf_tensorops PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) -add_library(cvcore STATIC +add_library(corelib STATIC # Tensorops - cvcore/src/tensor_ops/ArithmeticOperations.cpp - cvcore/src/tensor_ops/BBoxUtils.cpp - cvcore/src/tensor_ops/ColorConversions.cpp - cvcore/src/tensor_ops/DBScan.cpp - cvcore/src/tensor_ops/Errors.cpp - cvcore/src/tensor_ops/Filters.cpp - cvcore/src/tensor_ops/Filters.h - cvcore/src/tensor_ops/FusedOperations.cpp - cvcore/src/tensor_ops/GeometryTransforms.cpp - cvcore/src/tensor_ops/IImageWarp.cpp - cvcore/src/tensor_ops/NppUtils.cpp - cvcore/src/tensor_ops/NppUtils.h - cvcore/src/tensor_ops/OneEuroFilter.cpp - cvcore/src/tensor_ops/TensorOperators.cpp - - # Tensorops/VPI - cvcore/src/tensor_ops/vpi/VPIColorConvertImpl.cpp - cvcore/src/tensor_ops/vpi/VPIColorConvertImpl.h - cvcore/src/tensor_ops/vpi/VPIEnumMapping.h - cvcore/src/tensor_ops/vpi/VPIImageWarp.h - cvcore/src/tensor_ops/vpi/VPIRemapImpl.cpp - cvcore/src/tensor_ops/vpi/VPIRemapImpl.h - cvcore/src/tensor_ops/vpi/VPIResizeImpl.cpp - cvcore/src/tensor_ops/vpi/VPIResizeImpl.h - cvcore/src/tensor_ops/vpi/VPIStatusMapping.cpp - cvcore/src/tensor_ops/vpi/VPIStatusMapping.h - cvcore/src/tensor_ops/vpi/VPIStereoDisparityEstimatorImpl.cpp - cvcore/src/tensor_ops/vpi/VPIStereoDisparityEstimatorImpl.h - cvcore/src/tensor_ops/vpi/VPITensorOperators.cpp - cvcore/src/tensor_ops/vpi/VPITensorOperators.h - - # Core - cvcore/src/core/cvcore/Array.cpp - cvcore/src/core/cvcore/Dummy.cu - cvcore/src/core/cvcore/MathTypes.cpp - cvcore/src/core/cvcore/Tensor.cpp - cvcore/src/core/utility/CVError.cpp - cvcore/src/core/utility/Instrumentation.cpp - cvcore/src/core/utility/Memory.cpp - cvcore/src/core/utility/ProfileUtils.cpp + extensions/tensorops/core/ArithmeticOperations.cpp + extensions/tensorops/core/BBoxUtils.cpp + extensions/tensorops/core/ColorConversions.cpp + extensions/tensorops/core/DBScan.cpp + extensions/tensorops/core/Errors.cpp + extensions/tensorops/core/Filters.cpp + extensions/tensorops/core/FusedOperations.cpp + extensions/tensorops/core/GeometryTransforms.cpp + extensions/tensorops/core/IImageWarp.cpp + extensions/tensorops/core/OneEuroFilter.cpp + extensions/tensorops/core/TensorOperators.cpp + extensions/tensorops/core/VPIColorConvertImpl.cpp + extensions/tensorops/core/VPIRemapImpl.cpp + extensions/tensorops/core/VPIResizeImpl.cpp + extensions/tensorops/core/VPIStatusMapping.cpp + extensions/tensorops/core/VPIStereoDisparityEstimatorImpl.cpp + extensions/tensorops/core/VPITensorOperators.cpp + extensions/tensorops/core/BBoxUtils.h + extensions/tensorops/core/DBScan.h + extensions/tensorops/core/Errors.h + extensions/tensorops/core/Filters.h + extensions/tensorops/core/IImageWarp.h + extensions/tensorops/core/ITensorOperatorContext.h + extensions/tensorops/core/ITensorOperatorStream.h + extensions/tensorops/core/ImageUtils.h + extensions/tensorops/core/OneEuroFilter.h + extensions/tensorops/core/TensorOperators.h + extensions/tensorops/core/VPIColorConvertImpl.h + extensions/tensorops/core/VPIEnumMapping.h + extensions/tensorops/core/VPIImageWarp.h + extensions/tensorops/core/VPIRemapImpl.h + extensions/tensorops/core/VPIResizeImpl.h + extensions/tensorops/core/VPIStatusMapping.h + extensions/tensorops/core/VPIStereoDisparityEstimatorImpl.h + extensions/tensorops/core/VPITensorOperators.h + extensions/tensorops/core/NppUtils.cpp + extensions/tensorops/core/NppUtils.h + extensions/tensorops/core/Filters.cpp + extensions/tensorops/core/Filters.h + extensions/tensorops/core/Array.cpp + extensions/tensorops/core/CVError.cpp + extensions/tensorops/core/MathTypes.cpp + extensions/tensorops/core/Memory.cpp + extensions/tensorops/core/Tensor.cpp + extensions/tensorops/core/Array.h + extensions/tensorops/core/BBox.h + extensions/tensorops/core/CVError.h + extensions/tensorops/core/CameraModel.h + extensions/tensorops/core/ComputeEngine.h + extensions/tensorops/core/Core.h + extensions/tensorops/core/Image.h + extensions/tensorops/core/MathTypes.h + extensions/tensorops/core/Memory.h + extensions/tensorops/core/Tensor.h + extensions/tensorops/core/Traits.h ) -target_include_directories(cvcore PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/cvcore/include) -target_compile_definitions(cvcore PRIVATE -DENABLE_VPI=1) +target_include_directories(corelib PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) +# target_compile_definitions(corelib PRIVATE -DENABLE_VPI=1) -target_link_libraries(cvcore PUBLIC - CUDA::cudart - CUDA::nppc - CUDA::nppial - CUDA::nppicc - CUDA::nppidei - CUDA::nppif - CUDA::nppig - CUDA::nppisu - vpi +target_link_libraries(corelib + PUBLIC + CUDA::cudart + CUDA::nppc + CUDA::nppial + CUDA::nppicc + CUDA::nppidei + CUDA::nppif + CUDA::nppig + CUDA::nppisu + vpi ) target_link_libraries(gxf_tensorops @@ -132,5 +148,5 @@ target_link_libraries(gxf_tensorops GXF::multimedia yaml-cpp PRIVATE - cvcore + corelib ) diff --git a/isaac_ros_image_proc/gxf/tensorops/ConvertColorFormat.cpp b/isaac_ros_image_proc/gxf/tensorops/ConvertColorFormat.cpp deleted file mode 100644 index 074e013..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/ConvertColorFormat.cpp +++ /dev/null @@ -1,214 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 -#include "ConvertColorFormat.hpp" - -namespace nvidia { -namespace cvcore { -namespace tensor_ops { - -namespace detail { - -template<::cvcore::ImageType T_IN, ::cvcore::ImageType T_OUT> -gxf_result_t ConvertColorFormatImpl(gxf::Entity& output, gxf::Entity& input, const ImageInfo& output_info, - const ImageInfo& input_info, const char* output_name, const char* input_name, - gxf::Handle output_adapter, gxf::Handle input_adapter, - gxf::Handle allocator, - ::cvcore::tensor_ops::ColorConversionType type, cudaStream_t stream) { - auto input_image = input_adapter->WrapImageFromMessage(input, input_name); - if (!input_image) { - return GXF_FAILURE; - } - - auto error = output_adapter->AddImageToMessage(output, output_info.width, output_info.height, allocator, - output_info.is_cpu, output_name); - if (error != GXF_SUCCESS) { - return GXF_FAILURE; - } - - auto output_image = output_adapter->WrapImageFromMessage(output, output_name); - if (!output_image) { - return GXF_FAILURE; - } - - ::cvcore::tensor_ops::ConvertColorFormat(output_image.value(), input_image.value(), type, stream); - return GXF_SUCCESS; -} - -template<::cvcore::ImageType T_IN, ::cvcore::ImageType T_OUT> -gxf_result_t ConvertColorFormatStreamImpl(gxf::Entity& output, gxf::Entity& input, const ImageInfo& output_info, - const ImageInfo& input_info, const char* output_name, const char* input_name, - gxf::Handle stream, gxf::Handle output_adapter, - gxf::Handle input_adapter, - gxf::Handle allocator) { - auto input_image = input_adapter->WrapImageFromMessage(input, input_name); - if (!input_image) { - return GXF_FAILURE; - } - - auto error = output_adapter->AddImageToMessage(output, output_info.width, output_info.height, allocator, - output_info.is_cpu, output_name); - if (error != GXF_SUCCESS) { - return GXF_FAILURE; - } - - auto output_image = output_adapter->WrapImageFromMessage(output, output_name); - if (!output_image) { - return GXF_FAILURE; - } - - auto err_code = stream->getStream()->ColorConvert(output_image.value(), input_image.value()); - if (err_code != ::cvcore::make_error_condition(::cvcore::ErrorCode::SUCCESS)) { - GXF_LOG_ERROR("color conversion operation failed."); - return GXF_FAILURE; - } - - return GXF_SUCCESS; -} - -} // namespace detail - -template -gxf_result_t ConvertColorFormatBase::registerInterface(gxf::Registrar* registrar) { - gxf::Expected result; - - result &= registrar->parameter(output_type_, "output_type"); - result &= registrar->parameter(receiver_, "receiver"); - result &= registrar->parameter(transmitter_, "transmitter"); - result &= registrar->parameter(pool_, "pool"); - result &= registrar->parameter(stream_, "stream", "tensor stream", "tensor stream object", - gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); - result &= registrar->parameter(stream_pool_, "stream_pool", "cuda stream pool", "cuda stream pool object", - gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); - result &= registrar->parameter(input_adapter_, "input_adapter"); - result &= registrar->parameter(output_adapter_, "output_adapter"); - result &= registrar->parameter(input_name_, "input_name", "input name", "input tensor name", - gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); - result &= registrar->parameter(output_name_, "output_name", "output name", "output tensor name", - gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); - - return gxf::ToResultCode(result); -} - -template -gxf::Expected ConvertColorFormatBase::doInferOutputInfo(gxf::Entity& input) { - // Set output type - auto output_type = GetImageTypeFromString(output_type_); - if (!output_type) { - return gxf::Unexpected{GXF_FAILURE}; - } - // Check if no-op is needed - no_op_ = output_type.value() == input_info_.type; - return ImageInfo{output_type.value(), input_info_.width, input_info_.height, input_info_.is_cpu}; -} - -template -gxf_result_t ConvertColorFormatBase::doUpdateCameraMessage(gxf::Handle& output, - gxf::Handle& input) { - *output = *input; - return GXF_SUCCESS; -} - -#define DEFINE_CONVERT_COLOR_FORMAT(INPUT_TYPE, OUTPUT_TYPE, CONVERSION_TYPE) \ - if (input_info_.type == INPUT_TYPE && output_info_.type == OUTPUT_TYPE) { \ - return detail::ConvertColorFormatImpl( \ - output, input, output_info_, input_info_, output_name, input_name, output_adapter_.get(), input_adapter_.get(), \ - pool_.get(), CONVERSION_TYPE, stream); \ - } - -#define DEFINE_STREAM_CONVERT_COLOR_FORMAT(INPUT_TYPE, OUTPUT_TYPE) \ - if (input_info_.type == INPUT_TYPE && output_info_.type == OUTPUT_TYPE) { \ - return detail::ConvertColorFormatStreamImpl( \ - output, input, output_info_, input_info_, output_name, input_name, stream_.try_get().value(), \ - output_adapter_.get(), input_adapter_.get(), pool_.get()); \ - } - -template<> -gxf_result_t ConvertColorFormatBase::doExecute(gxf::Entity& output, gxf::Entity& input, cudaStream_t stream, - const char* output_name, const char* input_name) { - GXF_LOG_INFO("execute convert color format"); - - // Run the color conversion operation - DEFINE_STREAM_CONVERT_COLOR_FORMAT(::cvcore::ImageType::RGB_U8, ::cvcore::ImageType::BGR_U8); - DEFINE_STREAM_CONVERT_COLOR_FORMAT(::cvcore::ImageType::BGR_U8, ::cvcore::ImageType::RGB_U8); - DEFINE_STREAM_CONVERT_COLOR_FORMAT(::cvcore::ImageType::NV12, ::cvcore::ImageType::BGR_U8); - DEFINE_STREAM_CONVERT_COLOR_FORMAT(::cvcore::ImageType::BGR_U8, ::cvcore::ImageType::NV12); - DEFINE_STREAM_CONVERT_COLOR_FORMAT(::cvcore::ImageType::NV12, ::cvcore::ImageType::RGB_U8); - DEFINE_STREAM_CONVERT_COLOR_FORMAT(::cvcore::ImageType::RGB_U8, ::cvcore::ImageType::NV12); - DEFINE_STREAM_CONVERT_COLOR_FORMAT(::cvcore::ImageType::NV24, ::cvcore::ImageType::BGR_U8); - DEFINE_STREAM_CONVERT_COLOR_FORMAT(::cvcore::ImageType::BGR_U8, ::cvcore::ImageType::NV24); - DEFINE_STREAM_CONVERT_COLOR_FORMAT(::cvcore::ImageType::NV24, ::cvcore::ImageType::RGB_U8); - DEFINE_STREAM_CONVERT_COLOR_FORMAT(::cvcore::ImageType::RGB_U8, ::cvcore::ImageType::NV24); - - // Return error code for unsupported type - GXF_LOG_ERROR("invalid input/output type for image color conversion."); - return GXF_FAILURE; -} - -template<> -gxf_result_t ConvertColorFormatBase::doExecute(gxf::Entity& output, gxf::Entity& input, cudaStream_t stream, - const char* output_name, const char* input_name) { - GXF_LOG_INFO("execute convert color format"); - - // Run the color conversion operation - DEFINE_CONVERT_COLOR_FORMAT(::cvcore::ImageType::RGB_U8, ::cvcore::ImageType::BGR_U8, - ::cvcore::tensor_ops::ColorConversionType::RGB2BGR); - DEFINE_CONVERT_COLOR_FORMAT(::cvcore::ImageType::RGB_U16, ::cvcore::ImageType::BGR_U16, - ::cvcore::tensor_ops::ColorConversionType::RGB2BGR); - DEFINE_CONVERT_COLOR_FORMAT(::cvcore::ImageType::RGB_F32, ::cvcore::ImageType::BGR_F32, - ::cvcore::tensor_ops::ColorConversionType::RGB2BGR); - DEFINE_CONVERT_COLOR_FORMAT(::cvcore::ImageType::BGR_U8, ::cvcore::ImageType::RGB_U8, - ::cvcore::tensor_ops::ColorConversionType::BGR2RGB); - DEFINE_CONVERT_COLOR_FORMAT(::cvcore::ImageType::BGR_U16, ::cvcore::ImageType::RGB_U16, - ::cvcore::tensor_ops::ColorConversionType::BGR2RGB); - DEFINE_CONVERT_COLOR_FORMAT(::cvcore::ImageType::BGR_F32, ::cvcore::ImageType::RGB_F32, - ::cvcore::tensor_ops::ColorConversionType::BGR2RGB); - DEFINE_CONVERT_COLOR_FORMAT(::cvcore::ImageType::RGB_U8, ::cvcore::ImageType::Y_U8, - ::cvcore::tensor_ops::ColorConversionType::RGB2GRAY); - DEFINE_CONVERT_COLOR_FORMAT(::cvcore::ImageType::RGB_U16, ::cvcore::ImageType::Y_U16, - ::cvcore::tensor_ops::ColorConversionType::RGB2GRAY); - DEFINE_CONVERT_COLOR_FORMAT(::cvcore::ImageType::RGB_F32, ::cvcore::ImageType::Y_F32, - ::cvcore::tensor_ops::ColorConversionType::RGB2GRAY); - DEFINE_CONVERT_COLOR_FORMAT(::cvcore::ImageType::BGR_U8, ::cvcore::ImageType::Y_U8, - ::cvcore::tensor_ops::ColorConversionType::BGR2GRAY); - DEFINE_CONVERT_COLOR_FORMAT(::cvcore::ImageType::BGR_U16, ::cvcore::ImageType::Y_U16, - ::cvcore::tensor_ops::ColorConversionType::BGR2GRAY); - DEFINE_CONVERT_COLOR_FORMAT(::cvcore::ImageType::BGR_F32, ::cvcore::ImageType::Y_F32, - ::cvcore::tensor_ops::ColorConversionType::BGR2GRAY); - DEFINE_CONVERT_COLOR_FORMAT(::cvcore::ImageType::Y_U8, ::cvcore::ImageType::RGB_U8, - ::cvcore::tensor_ops::ColorConversionType::GRAY2RGB); - DEFINE_CONVERT_COLOR_FORMAT(::cvcore::ImageType::Y_U16, ::cvcore::ImageType::RGB_U16, - ::cvcore::tensor_ops::ColorConversionType::GRAY2RGB); - DEFINE_CONVERT_COLOR_FORMAT(::cvcore::ImageType::Y_F32, ::cvcore::ImageType::RGB_F32, - ::cvcore::tensor_ops::ColorConversionType::GRAY2RGB); - DEFINE_CONVERT_COLOR_FORMAT(::cvcore::ImageType::Y_U8, ::cvcore::ImageType::BGR_U8, - ::cvcore::tensor_ops::ColorConversionType::GRAY2BGR); - DEFINE_CONVERT_COLOR_FORMAT(::cvcore::ImageType::Y_U16, ::cvcore::ImageType::BGR_U16, - ::cvcore::tensor_ops::ColorConversionType::GRAY2BGR); - DEFINE_CONVERT_COLOR_FORMAT(::cvcore::ImageType::Y_F32, ::cvcore::ImageType::BGR_F32, - ::cvcore::tensor_ops::ColorConversionType::GRAY2BGR); - - // Return error code for unsupported type - GXF_LOG_ERROR("invalid input/output type for image color conversion."); - return GXF_FAILURE; -} - -template class ConvertColorFormatBase; -template class ConvertColorFormatBase; - -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/CropAndResize.cpp b/isaac_ros_image_proc/gxf/tensorops/CropAndResize.cpp deleted file mode 100644 index d0c2872..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/CropAndResize.cpp +++ /dev/null @@ -1,161 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 -#include "CropAndResize.hpp" -#include "Resize.hpp" - -namespace nvidia { -namespace cvcore { -namespace tensor_ops { - -namespace detail { - -template<::cvcore::ImageType T> -gxf_result_t CropAndResizeImpl(gxf::Entity& output, gxf::Entity& input, const ImageInfo& output_info, - const ImageInfo& input_info, const char* output_name, const char* input_name, - gxf::Handle output_adapter, gxf::Handle input_adapter, - gxf::Handle allocator, const std::vector<::cvcore::BBox>& src_rois, - ::cvcore::tensor_ops::InterpolationType interp_type, cudaStream_t stream) { - auto input_image = input_adapter->WrapImageFromMessage(input, input_name); - if (!input_image) { - return GXF_FAILURE; - } - - const size_t num_output = src_rois.size(); - - for (size_t i = 0; i < num_output; i++) { - const std::string output_name_i = std::string(output_name) + "_" + std::to_string(i); - auto error = output_adapter->AddImageToMessage(output, output_info.width, output_info.height, allocator, - output_info.is_cpu, output_name_i.c_str()); - if (error != GXF_SUCCESS) { - return GXF_FAILURE; - } - auto output_image = output_adapter->WrapImageFromMessage(output, output_name_i.c_str()); - if (!output_image) { - return GXF_FAILURE; - } - ::cvcore::tensor_ops::CropAndResize(output_image.value(), input_image.value(), src_rois[i], interp_type, stream); - } - - return GXF_SUCCESS; -} - -} // namespace detail - -gxf_result_t CropAndResize::registerInterface(gxf::Registrar* registrar) { - gxf::Expected result; - - result &= registrar->parameter(output_width_, "output_width"); - result &= registrar->parameter(output_height_, "output_height"); - result &= registrar->parameter(interp_type_, "interp_type"); - result &= registrar->parameter(keep_aspect_ratio_, "keep_aspect_ratio"); - result &= registrar->parameter(receiver_bbox_, "receiver_bbox"); - result &= registrar->parameter(receiver_, "receiver"); - result &= registrar->parameter(transmitter_, "transmitter"); - result &= registrar->parameter(pool_, "pool"); - result &= registrar->parameter(stream_pool_, "stream_pool", "cuda stream pool", "cuda stream pool object", - gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); - result &= registrar->parameter(input_adapter_, "input_adapter"); - result &= registrar->parameter(output_adapter_, "output_adapter"); - result &= registrar->parameter(input_name_, "input_name", "input name", "input tensor name", - gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); - result &= registrar->parameter(output_name_, "output_name", "output name", "output tensor name", - gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); - - return gxf::ToResultCode(result); -} - -gxf::Expected CropAndResize::doInferOutputInfo(gxf::Entity& input) { - // Set crop regions - auto input_bbox_message = receiver_bbox_->receive(); - if (!input_bbox_message) { - return gxf::Unexpected{GXF_FAILURE}; - } - auto bbox_tensor = input_bbox_message.value().get(); - if (!bbox_tensor) { - return gxf::Unexpected{GXF_FAILURE}; - } - const gxf::Shape bbox_shape = bbox_tensor.value()->shape(); - if (bbox_shape.rank() != 2 || bbox_shape.dimension(1) != 4) { - GXF_LOG_ERROR("invalid input bbox dimension."); - return gxf::Unexpected{GXF_FAILURE}; - } - const size_t num_bbox = bbox_shape.dimension(0); - auto bbox_pointer = bbox_tensor.value()->data(); - if (!bbox_pointer) { - GXF_LOG_ERROR("empty bbox input."); - return gxf::Unexpected{GXF_FAILURE}; - } - std::vector<::cvcore::BBox> rois; - for (size_t i = 0; i < num_bbox; i++) { - const int index = i * 4; - rois.push_back({bbox_pointer.value()[index], bbox_pointer.value()[index + 1], bbox_pointer.value()[index + 2], - bbox_pointer.value()[index + 3]}); - } - input_rois_ = std::move(rois); - // Check if no-op is needed - no_op_ = input_rois_.size() == 1 && input_rois_[0].xmin == 0 && - input_rois_[0].xmax == static_cast(input_info_.width) && input_rois_[0].ymin == 0 && - input_rois_[0].ymax == static_cast(input_info_.height); - - return ImageInfo{input_info_.type, output_width_.get(), output_height_.get(), input_info_.is_cpu}; -} - -gxf_result_t CropAndResize::doUpdateCameraMessage(gxf::Handle& output, - gxf::Handle& input) { - auto crop_result = GetCroppedCameraModel(*input, input_rois_[0]); - if (!crop_result) { - return GXF_FAILURE; - } - *output = GetScaledCameraModel(crop_result.value(), output_info_.width, output_info_.height, false).value(); - return GXF_SUCCESS; -} - -#define DEFINE_CROP_AND_RESIZE(INPUT_TYPE) \ - if (input_info_.type == INPUT_TYPE) { \ - return detail::CropAndResizeImpl(output, input, output_info_, input_info_, output_name, input_name, \ - output_adapter_.get(), input_adapter_.get(), pool_.get(), \ - input_rois_, interp.value(), stream); \ - } - -gxf_result_t CropAndResize::doExecute(gxf::Entity& output, gxf::Entity& input, cudaStream_t stream, - const char* output_name, const char* input_name) { - GXF_LOG_INFO("execute crop_and_resize."); - // Check if interpolation type is valid - auto interp = GetInterpolationType(interp_type_); - if (!interp) { - return interp.error(); - } - - // Run the image resizing operation - DEFINE_CROP_AND_RESIZE(::cvcore::ImageType::Y_U8); - DEFINE_CROP_AND_RESIZE(::cvcore::ImageType::Y_U16); - DEFINE_CROP_AND_RESIZE(::cvcore::ImageType::Y_F32); - DEFINE_CROP_AND_RESIZE(::cvcore::ImageType::RGB_U8); - DEFINE_CROP_AND_RESIZE(::cvcore::ImageType::RGB_U16); - DEFINE_CROP_AND_RESIZE(::cvcore::ImageType::RGB_F32); - DEFINE_CROP_AND_RESIZE(::cvcore::ImageType::BGR_U8); - DEFINE_CROP_AND_RESIZE(::cvcore::ImageType::BGR_U16); - DEFINE_CROP_AND_RESIZE(::cvcore::ImageType::BGR_F32); - - // Return error code for unsupported type - GXF_LOG_ERROR("invalid input/output type for image crop_and_resize."); - return GXF_FAILURE; -} - -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/InterleavedToPlanar.cpp b/isaac_ros_image_proc/gxf/tensorops/InterleavedToPlanar.cpp deleted file mode 100644 index 7b63825..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/InterleavedToPlanar.cpp +++ /dev/null @@ -1,146 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 -#include "InterleavedToPlanar.hpp" - -namespace nvidia { -namespace cvcore { -namespace tensor_ops { - -namespace detail { - -template<::cvcore::ImageType T_IN, ::cvcore::ImageType T_OUT> -gxf_result_t InterleavedToPlanarImpl(gxf::Entity& output, gxf::Entity& input, const ImageInfo& output_info, - const ImageInfo& input_info, const char* output_name, const char* input_name, - gxf::Handle output_adapter, gxf::Handle input_adapter, - gxf::Handle allocator, cudaStream_t stream) { - auto input_image = input_adapter->WrapImageFromMessage(input, input_name); - if (!input_image) { - return GXF_FAILURE; - } - - auto error = output_adapter->AddImageToMessage(output, output_info.width, output_info.height, allocator, - output_info.is_cpu, output_name); - if (error != GXF_SUCCESS) { - return GXF_FAILURE; - } - - auto output_image = output_adapter->WrapImageFromMessage(output, output_name); - if (!output_image) { - return GXF_FAILURE; - } - ::cvcore::tensor_ops::InterleavedToPlanar(output_image.value(), input_image.value(), stream); - return GXF_SUCCESS; -} - -} // namespace detail - -gxf_result_t InterleavedToPlanar::registerInterface(gxf::Registrar* registrar) { - gxf::Expected result; - - result &= registrar->parameter(receiver_, "receiver"); - result &= registrar->parameter(transmitter_, "transmitter"); - result &= registrar->parameter(pool_, "pool"); - result &= registrar->parameter(stream_pool_, "stream_pool", "cuda stream pool", "cuda stream pool object", - gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); - result &= registrar->parameter(input_adapter_, "input_adapter"); - result &= registrar->parameter(output_adapter_, "output_adapter"); - result &= registrar->parameter(input_name_, "input_name", "input name", "input tensor name", - gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); - result &= registrar->parameter(output_name_, "output_name", "output name", "output tensor name", - gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); - - return gxf::ToResultCode(result); -} - -gxf::Expected InterleavedToPlanar::doInferOutputInfo(gxf::Entity& input) { - // Output type is planar - ::cvcore::ImageType output_type; - switch (input_info_.type) { - case ::cvcore::ImageType::RGB_U8: { - output_type = ::cvcore::ImageType::PLANAR_RGB_U8; - break; - } - case ::cvcore::ImageType::RGB_U16: { - output_type = ::cvcore::ImageType::PLANAR_RGB_U16; - break; - } - case ::cvcore::ImageType::RGB_F32: { - output_type = ::cvcore::ImageType::PLANAR_RGB_F32; - break; - } - case ::cvcore::ImageType::BGR_U8: { - output_type = ::cvcore::ImageType::PLANAR_BGR_U8; - break; - } - case ::cvcore::ImageType::BGR_U16: { - output_type = ::cvcore::ImageType::PLANAR_BGR_U16; - break; - } - case ::cvcore::ImageType::BGR_F32: { - output_type = ::cvcore::ImageType::PLANAR_BGR_F32; - break; - } - case ::cvcore::ImageType::PLANAR_RGB_U8: - case ::cvcore::ImageType::PLANAR_RGB_U16: - case ::cvcore::ImageType::PLANAR_RGB_F32: - case ::cvcore::ImageType::PLANAR_BGR_U8: - case ::cvcore::ImageType::PLANAR_BGR_U16: - case ::cvcore::ImageType::PLANAR_BGR_F32: { - output_type = input_info_.type; - no_op_ = true; - break; - } - default: { - GXF_LOG_ERROR("invalid input type for interleaved to planar conversion."); - return gxf::Unexpected{GXF_FAILURE}; - } - } - return ImageInfo{output_type, input_info_.width, input_info_.height, input_info_.is_cpu}; -} - -gxf_result_t InterleavedToPlanar::doUpdateCameraMessage(gxf::Handle& output, - gxf::Handle& input) { - *output = *input; - return GXF_SUCCESS; -} - -#define DEFINE_INTERLEAVED_TO_PLANAR(INPUT_TYPE, OUTPUT_TYPE) \ - if (input_info_.type == INPUT_TYPE && output_info_.type == OUTPUT_TYPE) { \ - return detail::InterleavedToPlanarImpl(output, input, output_info_, input_info_, \ - output_name, input_name, output_adapter_.get(), \ - input_adapter_.get(), pool_.get(), stream); \ - } - -gxf_result_t InterleavedToPlanar::doExecute(gxf::Entity& output, gxf::Entity& input, cudaStream_t stream, - const char* output_name, const char* input_name) { - GXF_LOG_INFO("execute interleaved_to_planar conversion"); - // Run the interleaved to planar operation - DEFINE_INTERLEAVED_TO_PLANAR(::cvcore::ImageType::RGB_U8, ::cvcore::ImageType::PLANAR_RGB_U8); - DEFINE_INTERLEAVED_TO_PLANAR(::cvcore::ImageType::RGB_U16, ::cvcore::ImageType::PLANAR_RGB_U16); - DEFINE_INTERLEAVED_TO_PLANAR(::cvcore::ImageType::RGB_F32, ::cvcore::ImageType::PLANAR_RGB_F32); - DEFINE_INTERLEAVED_TO_PLANAR(::cvcore::ImageType::BGR_U8, ::cvcore::ImageType::PLANAR_BGR_U8); - DEFINE_INTERLEAVED_TO_PLANAR(::cvcore::ImageType::BGR_U16, ::cvcore::ImageType::PLANAR_BGR_U16); - DEFINE_INTERLEAVED_TO_PLANAR(::cvcore::ImageType::BGR_F32, ::cvcore::ImageType::PLANAR_BGR_F32); - - // Return error code for unsupported type - GXF_LOG_ERROR("invalid input/output type for image interleaved to planar conversion."); - return GXF_FAILURE; -} - -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/Normalize.cpp b/isaac_ros_image_proc/gxf/tensorops/Normalize.cpp deleted file mode 100644 index 2429abb..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/Normalize.cpp +++ /dev/null @@ -1,183 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 -#include "Normalize.hpp" - -namespace nvidia { -namespace cvcore { -namespace tensor_ops { - -namespace detail { - -template<::cvcore::ImageType T_IN, ::cvcore::ImageType T_OUT> -gxf_result_t NormalizeC1Impl(gxf::Entity& output, gxf::Entity& input, const ImageInfo& output_info, - const ImageInfo& input_info, const char* output_name, const char* input_name, - gxf::Handle output_adapter, gxf::Handle input_adapter, - gxf::Handle allocator, const std::vector& scales, - const std::vector& offsets, cudaStream_t stream) { - if (scales.size() != 1 || offsets.size() != 1) { - GXF_LOG_ERROR("invalid scales/offsets dimension"); - return GXF_FAILURE; - } - - auto input_image = input_adapter->WrapImageFromMessage(input, input_name); - if (!input_image) { - return GXF_FAILURE; - } - - auto error = output_adapter->AddImageToMessage(output, output_info.width, output_info.height, allocator, - output_info.is_cpu, output_name); - if (error != GXF_SUCCESS) { - return GXF_FAILURE; - } - - auto output_image = output_adapter->WrapImageFromMessage(output, output_name); - if (!output_image) { - return GXF_FAILURE; - } - ::cvcore::tensor_ops::Normalize(output_image.value(), input_image.value(), scales[0], offsets[0], stream); - return GXF_SUCCESS; -} - -template<::cvcore::ImageType T_IN, ::cvcore::ImageType T_OUT> -gxf_result_t NormalizeC3Impl(gxf::Entity& output, gxf::Entity& input, const ImageInfo& output_info, - const ImageInfo& input_info, const char* output_name, const char* input_name, - gxf::Handle output_adapter, gxf::Handle input_adapter, - gxf::Handle allocator, const std::vector& scales, - const std::vector& offsets, cudaStream_t stream) { - if (scales.size() != 3 || offsets.size() != 3) { - GXF_LOG_ERROR("invalid scales/offsets dimension"); - return GXF_FAILURE; - } - - auto input_image = input_adapter->WrapImageFromMessage(input, input_name); - if (!input_image) { - return GXF_FAILURE; - } - - auto error = output_adapter->AddImageToMessage(output, output_info.width, output_info.height, allocator, - output_info.is_cpu, output_name); - if (error != GXF_SUCCESS) { - return GXF_FAILURE; - } - - auto output_image = output_adapter->WrapImageFromMessage(output, output_name); - if (!output_image) { - return GXF_FAILURE; - } - const float scales_value[3] = {scales[0], scales[1], scales[2]}; - const float offsets_value[3] = {offsets[0], offsets[1], offsets[2]}; - ::cvcore::tensor_ops::Normalize(output_image.value(), input_image.value(), scales_value, offsets_value, stream); - return GXF_SUCCESS; -} - -} // namespace detail - -gxf_result_t Normalize::registerInterface(gxf::Registrar* registrar) { - gxf::Expected result; - - result &= registrar->parameter(scales_, "scales"); - result &= registrar->parameter(offsets_, "offsets"); - result &= registrar->parameter(receiver_, "receiver"); - result &= registrar->parameter(transmitter_, "transmitter"); - result &= registrar->parameter(pool_, "pool"); - result &= registrar->parameter(stream_pool_, "stream_pool", "cuda stream pool", "cuda stream pool object", - gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); - result &= registrar->parameter(input_adapter_, "input_adapter"); - result &= registrar->parameter(output_adapter_, "output_adapter"); - result &= registrar->parameter(input_name_, "input_name", "input name", "input tensor name", - gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); - result &= registrar->parameter(output_name_, "output_name", "output name", "output tensor name", - gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); - - return gxf::ToResultCode(result); -} - -gxf::Expected Normalize::doInferOutputInfo(gxf::Entity& input) { - // Output type is F32 - ::cvcore::ImageType output_type; - switch (input_info_.type) { - case ::cvcore::ImageType::Y_U8: - case ::cvcore::ImageType::Y_U16: - case ::cvcore::ImageType::Y_F32: { - output_type = ::cvcore::ImageType::Y_F32; - break; - } - case ::cvcore::ImageType::RGB_U8: - case ::cvcore::ImageType::RGB_U16: - case ::cvcore::ImageType::RGB_F32: { - output_type = ::cvcore::ImageType::RGB_F32; - break; - } - case ::cvcore::ImageType::BGR_U8: - case ::cvcore::ImageType::BGR_U16: - case ::cvcore::ImageType::BGR_F32: { - output_type = ::cvcore::ImageType::BGR_F32; - break; - } - default: { - GXF_LOG_ERROR("invalid input type for normalize."); - return gxf::Unexpected{GXF_FAILURE}; - } - } - // Operation must be performed under any condition - no_op_ = false; - return ImageInfo{output_type, input_info_.width, input_info_.height, input_info_.is_cpu}; -} - -gxf_result_t Normalize::doUpdateCameraMessage(gxf::Handle& output, - gxf::Handle& input) { - *output = *input; - return GXF_SUCCESS; -} - -#define DEFINE_NORMALIZE_C1(INPUT_TYPE, OUTPUT_TYPE) \ - if (input_info_.type == INPUT_TYPE && output_info_.type == OUTPUT_TYPE) { \ - return detail::NormalizeC1Impl(output, input, output_info_, input_info_, output_name, \ - input_name, output_adapter_.get(), input_adapter_.get(), \ - pool_.get(), scales_.get(), offsets_.get(), stream); \ - } - -#define DEFINE_NORMALIZE_C3(INPUT_TYPE, OUTPUT_TYPE) \ - if (input_info_.type == INPUT_TYPE && output_info_.type == OUTPUT_TYPE) { \ - return detail::NormalizeC3Impl(output, input, output_info_, input_info_, output_name, \ - input_name, output_adapter_.get(), input_adapter_.get(), \ - pool_.get(), scales_.get(), offsets_.get(), stream); \ - } - -gxf_result_t Normalize::doExecute(gxf::Entity& output, gxf::Entity& input, cudaStream_t stream, const char* output_name, - const char* input_name) { - GXF_LOG_INFO("execute normalize"); - - // Run the image normalization operation - DEFINE_NORMALIZE_C1(::cvcore::ImageType::Y_U8, ::cvcore::ImageType::Y_F32); - DEFINE_NORMALIZE_C1(::cvcore::ImageType::Y_U16, ::cvcore::ImageType::Y_F32); - DEFINE_NORMALIZE_C1(::cvcore::ImageType::Y_F32, ::cvcore::ImageType::Y_F32); - DEFINE_NORMALIZE_C3(::cvcore::ImageType::RGB_U8, ::cvcore::ImageType::RGB_F32); - DEFINE_NORMALIZE_C3(::cvcore::ImageType::RGB_U16, ::cvcore::ImageType::RGB_F32); - DEFINE_NORMALIZE_C3(::cvcore::ImageType::RGB_F32, ::cvcore::ImageType::RGB_F32); - DEFINE_NORMALIZE_C3(::cvcore::ImageType::BGR_U8, ::cvcore::ImageType::BGR_F32); - DEFINE_NORMALIZE_C3(::cvcore::ImageType::BGR_U16, ::cvcore::ImageType::BGR_F32); - DEFINE_NORMALIZE_C3(::cvcore::ImageType::BGR_F32, ::cvcore::ImageType::BGR_F32); - - // Return error code for unsupported type - GXF_LOG_ERROR("invalid input/output type for image normalize."); - return GXF_FAILURE; -} - -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/Resize.cpp b/isaac_ros_image_proc/gxf/tensorops/Resize.cpp deleted file mode 100644 index 943ac5a..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/Resize.cpp +++ /dev/null @@ -1,194 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 -#include "Resize.hpp" - -namespace nvidia { -namespace cvcore { -namespace tensor_ops { - -namespace detail { - -template<::cvcore::ImageType T> -gxf_result_t ResizeImpl(gxf::Entity& output, gxf::Entity& input, const ImageInfo& output_info, - const ImageInfo& input_info, const char* output_name, const char* input_name, - gxf::Handle output_adapter, gxf::Handle input_adapter, - gxf::Handle allocator, bool keep_aspect_ratio, - ::cvcore::tensor_ops::InterpolationType interp_type, cudaStream_t stream) { - auto input_image = input_adapter->WrapImageFromMessage(input, input_name); - if (!input_image) { - return GXF_FAILURE; - } - - auto error = output_adapter->AddImageToMessage(output, output_info.width, output_info.height, allocator, - output_info.is_cpu, output_name); - if (error != GXF_SUCCESS) { - return GXF_FAILURE; - } - - auto output_image = output_adapter->WrapImageFromMessage(output, output_name); - if (!output_image) { - return GXF_FAILURE; - } - ::cvcore::tensor_ops::Resize(output_image.value(), input_image.value(), keep_aspect_ratio, interp_type, stream); - return GXF_SUCCESS; -} - -template<::cvcore::ImageType T> -gxf_result_t ResizeStreamImpl(gxf::Entity& output, gxf::Entity& input, const ImageInfo& output_info, - const ImageInfo& input_info, const char* output_name, const char* input_name, - gxf::Handle stream, gxf::Handle output_adapter, - gxf::Handle input_adapter, gxf::Handle allocator, - ::cvcore::tensor_ops::InterpolationType interp_type, - ::cvcore::tensor_ops::BorderType border_type) { - auto input_image = input_adapter->WrapImageFromMessage(input, input_name); - if (!input_image) { - return GXF_FAILURE; - } - - auto error = output_adapter->AddImageToMessage(output, output_info.width, output_info.height, allocator, - output_info.is_cpu, output_name); - if (error != GXF_SUCCESS) { - return GXF_FAILURE; - } - - auto output_image = output_adapter->WrapImageFromMessage(output, output_name); - if (!output_image) { - return GXF_FAILURE; - } - - auto err_code = stream->getStream()->Resize(output_image.value(), input_image.value(), interp_type, border_type); - if (err_code != ::cvcore::make_error_condition(::cvcore::ErrorCode::SUCCESS)) { - GXF_LOG_ERROR("resize operation failed."); - return GXF_FAILURE; - } - - return GXF_SUCCESS; -} - -} // namespace detail - -template -gxf_result_t ResizeBase::registerInterface(gxf::Registrar* registrar) { - gxf::Expected result; - - result &= registrar->parameter(output_width_, "output_width"); - result &= registrar->parameter(output_height_, "output_height"); - result &= registrar->parameter(interp_type_, "interp_type"); - result &= registrar->parameter(border_type_, "border_type"); - result &= registrar->parameter(keep_aspect_ratio_, "keep_aspect_ratio"); - result &= registrar->parameter(receiver_, "receiver"); - result &= registrar->parameter(transmitter_, "transmitter"); - result &= registrar->parameter(pool_, "pool"); - result &= registrar->parameter(stream_, "stream", "tensor stream", "tensor stream object", - gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); - result &= registrar->parameter(stream_pool_, "stream_pool", "cuda stream pool", "cuda stream pool object", - gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); - result &= registrar->parameter(input_adapter_, "input_adapter"); - result &= registrar->parameter(output_adapter_, "output_adapter"); - result &= registrar->parameter(input_name_, "input_name", "input name", "input tensor name", - gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); - result &= registrar->parameter(output_name_, "output_name", "output name", "output tensor name", - gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); - - return gxf::ToResultCode(result); -} - -template -gxf::Expected ResizeBase::doInferOutputInfo(gxf::Entity& input) { - // Check if no-op is needed - no_op_ = output_width_.get() == input_info_.width && output_height_.get() == input_info_.height; - return ImageInfo{input_info_.type, output_width_.get(), output_height_.get(), input_info_.is_cpu}; -} - -template -gxf_result_t ResizeBase::doUpdateCameraMessage(gxf::Handle& output, - gxf::Handle& input) { - *output = GetScaledCameraModel(*input, output_info_.width, output_info_.height, keep_aspect_ratio_.get()).value(); - return GXF_SUCCESS; -} - -#define DEFINE_RESIZE(INPUT_TYPE) \ - if (input_info_.type == INPUT_TYPE) { \ - return detail::ResizeImpl(output, input, output_info_, input_info_, output_name, input_name, \ - output_adapter_.get(), input_adapter_.get(), pool_.get(), \ - keep_aspect_ratio_.get(), interp.value(), stream); \ - } - -#define DEFINE_STREAM_RESIZE(INPUT_TYPE) \ - if (input_info_.type == INPUT_TYPE) { \ - return detail::ResizeStreamImpl(output, input, output_info_, input_info_, output_name, input_name, \ - stream_.try_get().value(), output_adapter_.get(), \ - input_adapter_.get(), pool_.get(), interp.value(), border.value()); \ - } - -template<> -gxf_result_t ResizeBase::doExecute(gxf::Entity& output, gxf::Entity& input, cudaStream_t stream, - const char* output_name, const char* input_name) { - GXF_LOG_INFO("execute resize."); - // Check if interpolation type is valid - auto interp = GetInterpolationType(interp_type_); - if (!interp) { - return interp.error(); - } - auto border = GetBorderType(border_type_); - if (!border) { - return border.error(); - } - - // Run the image resizing operation - DEFINE_STREAM_RESIZE(::cvcore::ImageType::RGB_U8); - DEFINE_STREAM_RESIZE(::cvcore::ImageType::BGR_U8); - DEFINE_STREAM_RESIZE(::cvcore::ImageType::NV12); - DEFINE_STREAM_RESIZE(::cvcore::ImageType::NV24); - - // Return error code for unsupported type - GXF_LOG_ERROR("invalid input/output type for image resize."); - return GXF_FAILURE; -} - -template<> -gxf_result_t ResizeBase::doExecute(gxf::Entity& output, gxf::Entity& input, cudaStream_t stream, - const char* output_name, const char* input_name) { - GXF_LOG_INFO("execute resize."); - // Check if interpolation type is valid - auto interp = GetInterpolationType(interp_type_); - if (!interp) { - return interp.error(); - } - - // Run the image resizing operation - DEFINE_RESIZE(::cvcore::ImageType::Y_U8); - DEFINE_RESIZE(::cvcore::ImageType::Y_U16); - DEFINE_RESIZE(::cvcore::ImageType::Y_F32); - DEFINE_RESIZE(::cvcore::ImageType::RGB_U8); - DEFINE_RESIZE(::cvcore::ImageType::RGB_U16); - DEFINE_RESIZE(::cvcore::ImageType::RGB_F32); - DEFINE_RESIZE(::cvcore::ImageType::BGR_U8); - DEFINE_RESIZE(::cvcore::ImageType::BGR_U16); - DEFINE_RESIZE(::cvcore::ImageType::BGR_F32); - - // Return error code for unsupported type - GXF_LOG_ERROR("invalid input/output type for image resize."); - return GXF_FAILURE; -} - -template class ResizeBase; -template class ResizeBase; - -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/TensorOps.cpp b/isaac_ros_image_proc/gxf/tensorops/TensorOps.cpp deleted file mode 100644 index 1694d95..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/TensorOps.cpp +++ /dev/null @@ -1,75 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 -#include "CameraModel.hpp" -#include "ConvertColorFormat.hpp" -#include "CropAndResize.hpp" -#include "Frame3D.hpp" -#include "ImageAdapter.hpp" -#include "InterleavedToPlanar.hpp" -#include "Normalize.hpp" -#include "Reshape.hpp" -#include "Resize.hpp" -#include "TensorOperator.hpp" -#include "TensorStream.hpp" -#include "Undistort.hpp" - -#include "gxf/std/extension_factory_helper.hpp" - -GXF_EXT_FACTORY_BEGIN() -GXF_EXT_FACTORY_SET_INFO(0x6eae64ff97a94d9b, 0xb324f85e6a98a75a, "NvCvTensorOpsExtension", - "Generic CVCORE tensor_ops interfaces", "Nvidia_Gxf", "3.1.0", "LICENSE"); - -GXF_EXT_FACTORY_ADD(0xd073a92344ba4b81, 0xbd0f18f4996048e9, nvidia::cvcore::tensor_ops::CameraModel, - nvidia::gxf::Component, - "Construct Camera distortion model / Camera intrinsic compatible with CVCORE"); - -GXF_EXT_FACTORY_ADD(0x6c9419223e4b4c2b, 0x899a4d65279c6507, nvidia::cvcore::tensor_ops::Frame3D, nvidia::gxf::Component, - "Construct Camera extrinsic compatible with CVCORE"); - -GXF_EXT_FACTORY_ADD(0xd94385e5b35b4634, 0x9adb0d214a3865f6, nvidia::cvcore::tensor_ops::TensorStream, - nvidia::gxf::Component, "Wrapper of CVCORE ITensorOperatorStream/ITensorOperatorContext"); - -GXF_EXT_FACTORY_ADD(0xd0c4ddad486a4a91, 0xb69c8a5304b205ef, nvidia::cvcore::tensor_ops::ImageAdapter, - nvidia::gxf::Component, "Utility component for conversion between message and cvcore image type"); - -GXF_EXT_FACTORY_ADD(0xadebc792bd0b4a56, 0x99c1405fd2ea0727, nvidia::cvcore::tensor_ops::StreamUndistort, - nvidia::gxf::Codelet, "Codelet for stream image undistortion in tensor_ops"); - -GXF_EXT_FACTORY_ADD(0xa58141ac7eca4ea5, 0x9b545446fe379a11, nvidia::cvcore::tensor_ops::Resize, nvidia::gxf::Codelet, - "Codelet for image resizing in tensor_ops"); - -GXF_EXT_FACTORY_ADD(0xeb8b5f5b36d44b48, 0x81f959fd28e6f677, nvidia::cvcore::tensor_ops::StreamResize, - nvidia::gxf::Codelet, "Codelet for stream image resizing in tensor_ops"); - -GXF_EXT_FACTORY_ADD(0x4a7ff422de3841bc, 0x9e743ac10d9294b6, nvidia::cvcore::tensor_ops::CropAndResize, - nvidia::gxf::Codelet, "Codelet for crop and resizing operation in tensor_ops"); - -GXF_EXT_FACTORY_ADD(0x7018f0b9034c462b, 0xa9fbaf7ee012974f, nvidia::cvcore::tensor_ops::Normalize, nvidia::gxf::Codelet, - "Codelet for image normalization in tensor_ops"); - -GXF_EXT_FACTORY_ADD(0x269d4237f3c3479d, 0xbcca9ecc44c71a70, nvidia::cvcore::tensor_ops::InterleavedToPlanar, - nvidia::gxf::Codelet, "Codelet for convert interleaved image to planar image in tensor_ops"); - -GXF_EXT_FACTORY_ADD(0xfc4d7b4d8fcc4daa, 0xa286056e0fcafa78, nvidia::cvcore::tensor_ops::ConvertColorFormat, - nvidia::gxf::Codelet, "Codelet for image color conversion in tensor_ops"); - -GXF_EXT_FACTORY_ADD(0x5ab4a4d8f7a34552, 0xa90be52660b076fd, nvidia::cvcore::tensor_ops::StreamConvertColorFormat, - nvidia::gxf::Codelet, "Codelet for stream image color conversion in tensor_ops"); - -GXF_EXT_FACTORY_ADD(0x26789b7d5a8d4e84, 0x86b845ec5f4cd12a, nvidia::cvcore::tensor_ops::Reshape, nvidia::gxf::Codelet, - "Codelet for image reshape in tensor_ops"); -GXF_EXT_FACTORY_END() diff --git a/isaac_ros_image_proc/gxf/tensorops/Undistort.cpp b/isaac_ros_image_proc/gxf/tensorops/Undistort.cpp deleted file mode 100644 index b1eed4e..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/Undistort.cpp +++ /dev/null @@ -1,285 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 - -#include - -#include "ImageUtils.hpp" -#include "Undistort.hpp" -#include "gxf/multimedia/camera.hpp" - -namespace nvidia { -namespace cvcore { -namespace tensor_ops { - -namespace detail { - -template<::cvcore::ImageType T> -gxf_result_t UndistortImpl(gxf::Entity& output, gxf::Entity& input, const ImageInfo& output_info, - const ImageInfo& input_info, const char* output_name, const char* input_name, - gxf::Handle stream, gxf::Handle output_adapter, - gxf::Handle input_adapter, gxf::Handle allocator, - ::cvcore::tensor_ops::ImageWarp warp, ::cvcore::tensor_ops::InterpolationType interp_type, - ::cvcore::tensor_ops::BorderType border_type) { - auto input_image = input_adapter->WrapImageFromMessage(input, input_name); - if (!input_image) { - return GXF_FAILURE; - } - - auto error = output_adapter->AddImageToMessage(output, output_info.width, output_info.height, allocator, - output_info.is_cpu, output_name); - if (error != GXF_SUCCESS) { - return GXF_FAILURE; - } - - auto output_image = output_adapter->WrapImageFromMessage(output, output_name); - if (!output_image) { - return GXF_FAILURE; - } - - auto err_code = stream->getStream()->Remap(output_image.value(), input_image.value(), warp, interp_type, border_type); - if (err_code != ::cvcore::make_error_condition(::cvcore::ErrorCode::SUCCESS)) { - GXF_LOG_ERROR("undistort operation failed."); - return GXF_FAILURE; - } - - return GXF_SUCCESS; -} - -gxf::Expected<::cvcore::CameraIntrinsics> GetIntrinsicsFromMessage(gxf::Handle& camera_model) { - return ::cvcore::CameraIntrinsics(camera_model->focal_length.x, camera_model->focal_length.y, - camera_model->principal_point.x, camera_model->principal_point.y, - camera_model->skew_value); -} - -gxf::Expected<::cvcore::CameraExtrinsics> GetExtrinsicsFromMessage(gxf::Handle& pose) { - float raw_matrix[3][4]; - for (size_t i = 0; i < 9; i++) { - raw_matrix[i / 3][i % 3] = pose->rotation[i]; - } - for (size_t i = 0; i < 3; i++) { - raw_matrix[i][3] = pose->translation[i]; - } - return ::cvcore::CameraExtrinsics(raw_matrix); -} - -gxf::Expected<::cvcore::CameraDistortionModel> GetDistortionsFromMessage(gxf::Handle& camera_model) { - auto distortion_type = GetCameraDistortionType(camera_model->distortion_type); - if (!distortion_type) { - return gxf::Unexpected{GXF_FAILURE}; - } - auto distortion_model = ::cvcore::CameraDistortionModel(); - for (size_t i = 0; i < 8; i++) { - distortion_model.coefficients[i] = camera_model->distortion_coefficients[i]; - } - distortion_model.type = distortion_type.value(); - return distortion_model; -} - -} // namespace detail - -gxf_result_t UndistortBase::start() { - // Load grid object - image_grid_.numHorizRegions = regions_width_.get().size(); - image_grid_.numVertRegions = regions_height_.get().size(); - if (regions_width_.get().size() != horizontal_intervals_.get().size() || - regions_height_.get().size() != vertical_intervals_.get().size()) { - GXF_LOG_ERROR("invalid image grid."); - return GXF_FAILURE; - } - std::copy(regions_width_.get().begin(), regions_width_.get().end(), image_grid_.regionWidth.begin()); - std::copy(regions_height_.get().begin(), regions_height_.get().end(), image_grid_.regionHeight.begin()); - std::copy(horizontal_intervals_.get().begin(), horizontal_intervals_.get().end(), image_grid_.horizInterval.begin()); - std::copy(vertical_intervals_.get().begin(), vertical_intervals_.get().end(), image_grid_.vertInterval.begin()); - output_shape_.x = static_cast( - std::accumulate(image_grid_.regionWidth.begin(), image_grid_.regionWidth.end(), 0)); - output_shape_.y = static_cast( - std::accumulate(image_grid_.regionHeight.begin(), image_grid_.regionHeight.end(), 0)); - - // Generate Image Warp if possible - if (input_camera_model_.try_get() && reference_frame_.try_get()) { - input_camera_info_ = {input_camera_model_.try_get().value()->getCameraIntrinsics(), - reference_frame_.try_get().value()->getCameraExtrinsics(), - input_camera_model_.try_get().value()->getDistortionModel()}; - - output_camera_intrinsics_ = output_camera_model_.try_get() - ? output_camera_model_.try_get().value()->getCameraIntrinsics() - : input_camera_info_.intrinsic; - - auto err_code = stream_->getStream()->GenerateWarpFromCameraModel(image_warp_, image_grid_, input_camera_info_, - output_camera_intrinsics_); - if (err_code != ::cvcore::make_error_condition(::cvcore::ErrorCode::SUCCESS)) { - GXF_LOG_ERROR("image warp creation failed."); - return GXF_FAILURE; - } - } - - return GXF_SUCCESS; -} - -gxf_result_t UndistortBase::stop() { - auto err_code = stream_->getStream()->DestroyWarp(image_warp_); - if (err_code != ::cvcore::make_error_condition(::cvcore::ErrorCode::SUCCESS)) { - GXF_LOG_ERROR("image warp de-allocation failed."); - return GXF_FAILURE; - } - return GXF_SUCCESS; -} - -gxf_result_t UndistortBase::registerInterface(gxf::Registrar* registrar) { - gxf::Expected result; - - result &= registrar->parameter(input_camera_model_, "input_camera_model", "", "", - gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); - result &= registrar->parameter(reference_frame_, "reference_frame", "", "", gxf::Registrar::NoDefaultParameter(), - GXF_PARAMETER_FLAGS_OPTIONAL); - result &= registrar->parameter(output_camera_model_, "output_camera_model", "", "", - gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); - result &= registrar->parameter(regions_width_, "regions_width"); - result &= registrar->parameter(regions_height_, "regions_height"); - result &= registrar->parameter(horizontal_intervals_, "horizontal_intervals"); - result &= registrar->parameter(vertical_intervals_, "vertical_intervals"); - result &= registrar->parameter(interp_type_, "interp_type"); - result &= registrar->parameter(border_type_, "border_type"); - result &= registrar->parameter(receiver_, "receiver"); - result &= registrar->parameter(transmitter_, "transmitter"); - result &= registrar->parameter(pool_, "pool"); - result &= registrar->parameter(stream_, "stream"); - result &= registrar->parameter(stream_pool_, "stream_pool", "cuda stream pool", "cuda stream pool object", - gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); - result &= registrar->parameter(input_adapter_, "input_adapter"); - result &= registrar->parameter(output_adapter_, "output_adapter"); - result &= registrar->parameter(input_name_, "input_name", "input name", "input tensor name", - gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); - result &= registrar->parameter(output_name_, "output_name", "output name", "output tensor name", - gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); - - return gxf::ToResultCode(result); -} - -gxf::Expected UndistortBase::doInferOutputInfo(gxf::Entity& input) { - // Check if the input distortion type is Perpective - auto maybe_camera_message = input.get(); - if (maybe_camera_message) { - no_op_ = maybe_camera_message.value()->distortion_type == gxf::DistortionType::Perspective; - } - // Output size may vary, but the format must be the same - return ImageInfo{input_info_.type, static_cast(output_shape_.x), static_cast(output_shape_.y), - input_info_.is_cpu}; -} - -gxf_result_t UndistortBase::doUpdateCameraMessage(gxf::Handle& output, - gxf::Handle& input) { - *output = *input; - (*output).distortion_type = gxf::DistortionType::Perspective; - for (size_t i = 0; i < gxf::CameraModel::kMaxDistortionCoefficients; i++) { - (*output).distortion_coefficients[i] = 0.; - } - (*output).dimensions = output_shape_; - (*output).focal_length.x = output_camera_intrinsics_.fx(); - (*output).focal_length.y = output_camera_intrinsics_.fy(); - (*output).principal_point.x = output_camera_intrinsics_.cx(); - (*output).principal_point.y = output_camera_intrinsics_.cy(); - (*output).skew_value = output_camera_intrinsics_.skew(); - return GXF_SUCCESS; -} - -#define DEFINE_UNDISTORT(INPUT_TYPE) \ - if (input_info_.type == INPUT_TYPE) { \ - return detail::UndistortImpl(output, input, output_info_, input_info_, output_name, input_name, \ - stream_.get(), output_adapter_.get(), input_adapter_.get(), pool_.get(), \ - image_warp_, interp.value(), border.value()); \ - } - -gxf_result_t UndistortBase::doExecute(gxf::Entity& output, gxf::Entity& input, cudaStream_t stream, - const char* output_name, const char* input_name) { - GXF_LOG_INFO("execute undistort."); - auto maybe_camera_message = input.get(); - auto maybe_pose3d_message = input.get(); - if (!maybe_camera_message) { - if (image_warp_ == nullptr) { - GXF_LOG_ERROR("no camera information found."); - return GXF_FAILURE; - } - } else { - auto maybe_intrinsics = detail::GetIntrinsicsFromMessage(maybe_camera_message.value()); - auto maybe_distortions = detail::GetDistortionsFromMessage(maybe_camera_message.value()); - if (!maybe_intrinsics || !maybe_distortions) { - return GXF_FAILURE; - } - const auto& new_intrinsics = maybe_intrinsics.value(); - const auto& new_distortions = maybe_distortions.value(); - - auto new_extrinsics = maybe_pose3d_message ? detail::GetExtrinsicsFromMessage(maybe_pose3d_message.value()).value() - : ::cvcore::CameraExtrinsics(); - - const bool reset = image_warp_ == nullptr || new_intrinsics != input_camera_info_.intrinsic || - new_distortions != input_camera_info_.distortion || - new_extrinsics != input_camera_info_.extrinsic; - - if (reset) { - auto new_width = static_cast(output_shape_.x); - auto new_height = static_cast(output_shape_.y); - - // These two parameters (width_scale and height_scale) can be - // used to determine a crop or pad regime depending on which dimension to - // preserve in the case of keep_aspect ratio. In this case, we assume - // always_crop=True, or that we will always use the largest dimension - // change. - auto width_scale = new_width / static_cast(input_info_.width); - auto height_scale = new_height / static_cast(input_info_.height); - auto scale = std::max({width_scale, height_scale}); // Always crop - - input_camera_info_ = {new_intrinsics, new_extrinsics, new_distortions}; - output_camera_intrinsics_ = new_intrinsics; - output_camera_intrinsics_.m_intrinsics[0][0] = scale * new_intrinsics.m_intrinsics[0][0]; - output_camera_intrinsics_.m_intrinsics[0][1] = scale * new_intrinsics.m_intrinsics[0][1]; - output_camera_intrinsics_.m_intrinsics[1][1] = scale * new_intrinsics.m_intrinsics[1][1]; - output_camera_intrinsics_.m_intrinsics[0][2] = scale * new_intrinsics.m_intrinsics[0][2]; - output_camera_intrinsics_.m_intrinsics[1][2] = scale * new_intrinsics.m_intrinsics[1][2]; - - auto err_code = stream_->getStream()->GenerateWarpFromCameraModel(image_warp_, image_grid_, input_camera_info_, - output_camera_intrinsics_); - if (err_code != ::cvcore::make_error_condition(::cvcore::ErrorCode::SUCCESS)) { - GXF_LOG_ERROR("image warp creation failed."); - return GXF_FAILURE; - } - } - } - - auto interp = GetInterpolationType(interp_type_); - if (!interp) { - return interp.error(); - } - auto border = GetBorderType(border_type_); - if (!border) { - return border.error(); - } - - // Run the image undistortion operation - DEFINE_UNDISTORT(::cvcore::ImageType::BGR_U8); - DEFINE_UNDISTORT(::cvcore::ImageType::RGB_U8); - DEFINE_UNDISTORT(::cvcore::ImageType::NV12); - DEFINE_UNDISTORT(::cvcore::ImageType::NV24); - - // Return error code for unsupported type - GXF_LOG_ERROR("invalid input/output type for image undistort."); - return GXF_FAILURE; -} - -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/Instrumentation.h b/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/Instrumentation.h deleted file mode 100644 index 6324b8d..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/Instrumentation.h +++ /dev/null @@ -1,65 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 - -#ifndef CVCORE_INSTRUMENTATION_H -#define CVCORE_INSTRUMENTATION_H - -#include - -namespace cvcore { namespace profiler { - -/** - * A enum class used to find out the type of profiler output required - */ -enum class ProfilerJsonOutputType : uint32_t -{ - JSON_OFF, /**< print the aggregate values of each timer in pretty print format */ - JSON_AGGREGATE, /**< print the aggregate values of each timer in JSON format - along with the pretty print format. Pretty print format - gets printed on the terminal */ - JSON_SEPARATE /**< print all the elapsed times for all timers along with the - aggregate values from JSON_AGGREGATE option */ -}; - -/** -* Flush call to print the timer values in a file input -* @param jsonHelperType used to find out the type of profiler output required -* @return filename used to write the timer values -*/ -void flush(const std::string& filename, ProfilerJsonOutputType jsonHelperType); - -/** -* Flush call to print the timer values in a output stream -* @param jsonHelperType used to find out the type of profiler output required -* @return output stream used to write the timer values -*/ -void flush(std::ostream& output, ProfilerJsonOutputType jsonHelperType); - -/** -* Flush call to print the timer values on the terminal -* @param jsonHelperType used to find out the type of profiler output required -*/ -void flush(ProfilerJsonOutputType jsonHelperType); - - -/** -* Clear all the profile timers -*/ -void clear(); - -}} -#endif diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/Model.h b/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/Model.h deleted file mode 100644 index 4a14945..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/Model.h +++ /dev/null @@ -1,50 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2020-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 - -#ifndef CVCORE_MODEL_H -#define CVCORE_MODEL_H - -#include - -#include "Image.h" - -namespace cvcore { - -/** - * Struct to describe input type required by the model - */ -struct ModelInputParams -{ - size_t maxBatchSize; /**< maxbatchSize supported by network*/ - size_t inputLayerWidth; /**< Input layer width */ - size_t inputLayerHeight; /**< Input layer Height */ - ImageType modelInputType; /**< Input Layout type */ -}; - -/** - * Struct to describe the model - */ -struct ModelInferenceParams -{ - std::string engineFilePath; /**< Engine file path. */ - std::vector inputLayers; /**< names of input layers. */ - std::vector outputLayers; /**< names of output layers. */ -}; - -} // namespace cvcore - -#endif // CVCORE_MODEL_H diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/ProfileUtils.h b/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/ProfileUtils.h deleted file mode 100644 index 0a4e9a5..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/ProfileUtils.h +++ /dev/null @@ -1,40 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2020-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 - -#ifndef CVCORE_PROFILE_UTILS_H -#define CVCORE_PROFILE_UTILS_H - -#include - -namespace cvcore { - -/** - * Export one profiling item to specified json file. - * @param outputPath output json file path. - * @param taskName item name showing in the output json file. - * @param tMin minimum running time in milliseconds. - * @param tMax maximum running time in milliseconds. - * @param tAvg average running time in milliseconds. - * @param isCPU whether CPU or GPU time. - * @param iterations number of iterations. - */ -void ExportToJson(const std::string outputPath, const std::string taskName, float tMin, float tMax, float tAvg, - bool isCPU, int iterations = 100); - -} // namespace cvcore - -#endif // CVCORE_PROFILE_UTILS_H diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/TensorMap.h b/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/TensorMap.h deleted file mode 100644 index 93c8783..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/TensorMap.h +++ /dev/null @@ -1,534 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2019-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 - -#ifndef CVCORE_TENSORMAP_H -#define CVCORE_TENSORMAP_H - -#include -#include -#include -#include -#include - -#include "Traits.h" -#include "TensorList.h" - -namespace cvcore { - -/** - * @brief Implementation of a map of tensors of the same rank but, potentially, different dimensions, over the batch dimension - * - * @tparam TensorType Any CVCORE tensor type - * @tparam KeyType Any STL hashable data type - */ -template -class TensorMap {}; - -template -class TensorMap, KT, - typename std::enable_if>::value>::type> -{ - using my_type = TensorMap, KT>; - - public: - using key_type = KT; - using unit_type = Tensor; - using element_type = traits::remove_batch_t; - using frame_type = TensorList; - using buffer_type = TensorList; - - template - struct dim_data_type - { - std::size_t height; - std::size_t width; - }; - - template - struct dim_data_type::type> - { - std::size_t height; - std::size_t width; - std::size_t channels; - }; - - TensorMap() = default; - TensorMap(const my_type &) = delete; - TensorMap(my_type && other) - { - *this = std::move(other); - } - - /** - * @brief Construct a new Tensor Map object - * - * @param batchSize The batch dimension of all sub-tensors - * @param dimData The dimensional description of all sub-tensors in at least HWC format - * @param isCPU A boolean flag specifying what device to allocate the sub-tensors - */ - template::type> - TensorMap(std::size_t batchSize, - const std::vector> & dimData, - bool isCPU = true) - : m_maxBatchSize{batchSize}, m_size{dimData.size()}, - m_isCPU{isCPU}, m_buffer{dimData.size(), true} - { - m_buffer.setSize(m_size); - - int i = 0; - for(auto dim: dimData) - { - m_buffer[i] = std::move(unit_type(dim.width, - dim.height, - m_maxBatchSize, - m_isCPU)); - ++i; - } - - for(std::size_t i = 0; i < m_maxBatchSize; ++i) - { - m_pool.insert(i); - } - } - - /** - * @brief Construct a new Tensor Map object - * - * @param batchSize The batch dimension of all sub-tensors - * @param dimData The dimensional description of all sub-tensors in at least HWC format - * @param isCPU A boolean flag specifying what device to allocate the sub-tensors - */ - template::type> - TensorMap(std::size_t batchSize, - const std::vector> & dimData, - bool isCPU = true) - : m_maxBatchSize{batchSize}, m_size{dimData.size()}, - m_isCPU{isCPU}, m_buffer{dimData.size(), true} - { - m_buffer.setSize(m_size); - - int i = 0; - for(auto dim: dimData) - { - m_buffer[i] = std::move(unit_type(dim.width, - dim.height, - m_maxBatchSize, - dim.channels, - m_isCPU)); - ++i; - } - - for(std::size_t i = 0; i < m_maxBatchSize; ++i) - { - m_pool.insert(i); - } - } - - ~TensorMap() = default; - - my_type & operator=(const my_type &) = delete; - my_type & operator=(my_type && other) - { - std::swap(m_mapping, other.m_mapping); - std::swap(m_pool, other.m_pool); - std::swap(m_maxBatchSize, other.m_maxBatchSize); - std::swap(m_size, other.m_size); - std::swap(m_isCPU, other.m_isCPU); - std::swap(m_buffer, other.m_buffer); - - return *this; - } - - /** - * @brief A mapping of the batch dimension index to a given key - * - * @details Given a set of pairs such that the keys AND values are unique - * respectively, the key-wise mapping of the batch dimension is reset - * to the provided values. - * - * @param pairs An unordered map of the uniqe key value pairs - * @return true If the length of ``pairs`` is less than the max batch size - * and the key value pairs are one-to-one and onto. - * @return false If the conditions of ``true`` are not met. - */ - bool remap(const std::unordered_map & pairs) - { - bool result = false; - - if(pairs.size() <= m_maxBatchSize) - { - for(std::size_t i = 0; i < m_maxBatchSize; ++i) - { - m_pool.insert(i); - } - - m_mapping.clear(); - for(auto mapping: pairs) - { - if(m_pool.erase(mapping.second)) - { - m_mapping[mapping.first] = mapping.second; - } - } - - if((pairs.size() + m_pool.size()) == m_maxBatchSize) - { - result = true; - } - } - - return result; - } - - /** - * @brief Associates a given key with the first available batch index - * - * @details Assuming the associated keys has not reached `maxBatchSize`` - * then this function associates a given key with the first available - * batch index and returns that index value. If no batch index is - * available -1 is returned. NOTE: if ``key`` is already associated - * with a batch index, the that index is returned. - * - * @param key The key to be associated with a batch index value - * @return std::intmax_t The batch index associated with the key or -1 - * if no index is available. NOTE: because std::intmax_t is not a full - * covering of std::size_t, it is possible for wrap around to happen. - */ - std::intmax_t map(const key_type & key) - { - auto it = m_mapping.find(key); - - if(it == m_mapping.end() && !m_pool.empty()) - { - auto value = m_pool.begin(); - it = m_mapping.insert({key, *value}).first; - m_pool.erase(value); - } - - return static_cast(it != m_mapping.end() ? it->second : -1); - } - - /** - * @brief Dissociates a given key with a batch index if possible - * - * @details Assuming the given key is associated with a batch index this - * function removes the association and returns the batch index is was - * associated with. If no batch index is found associated with the given - * key, -1 is returned. - * - * @param key The key to be dissociated - * @return std::intmax_t The batch index associated with the key or -1 - * if not found. NOTE: because std::intmax_t is not a full covering of - * std::size_t, it is possible for wrap around to happen. - */ - std::intmax_t unmap(const key_type & key) - { - std::intmax_t result = -1; - - auto it = m_mapping.find(key); - - if(it != m_mapping.end()) - { - result = static_cast(it->second); - m_pool.insert(it->second); - m_mapping.erase(it); - } - - return result; - } - - /** - * @brief The number of keys associated with a batch index - * - * @return std::size_t - */ - std::size_t getKeyCount() const noexcept - { - return m_mapping.size(); - } - - /** - * @brief The maximum number of batch index - * - * @return std::size_t - */ - std::size_t getMaxBatchSize() const noexcept - { - return m_maxBatchSize; - } - - /** - * @brief The number of sub-tensors - * - * @return std::size_t - */ - std::size_t getUnitCount() const - { - return m_size; - } - - /** - * Get the size of given dimension. - * @param dimIdx dimension index. - * @return size of the specified dimension. - */ - std::size_t getTensorSize(std::size_t unitIdx, std::size_t dimIdx) const - { - return m_buffer[unitIdx].getSize(dimIdx); - } - - /** - * Get the stride of given dimension. - * @param dimIdx dimension index. - * @return stride of the specified dimension. - */ - std::size_t getTensorStride(std::size_t unitIdx, std::size_t dimIdx) const - { - return m_buffer[unitIdx].getStride(dimIdx); - } - - template::type> - unit_type getUnit(std::size_t idx) - { - unit_type result{m_buffer[idx].getWidth(), - m_buffer[idx].getHeight(), - m_buffer[idx].getDepth(), - m_buffer[idx].getData(), - m_buffer[idx].isCPU()}; - return result; - } - - template::type> - unit_type getUnit(std::size_t idx, ChannelCount UNUSED = T) - { - unit_type result{m_buffer[idx].getWidth(), - m_buffer[idx].getHeight(), - m_buffer[idx].getDepth(), - m_buffer[idx].getChannelCount(), - m_buffer[idx].getData(), - m_buffer[idx].isCPU()}; - return result; - } - - template::type> - frame_type getFrame(const key_type & idx) - { - frame_type result; - - if(m_mapping.find(idx) != m_mapping.end()) - { - std::size_t at = m_mapping[idx]; - result = std::move(frame_type{m_buffer.getSize(), m_buffer.isCPU()}); - result.setSize(m_size); - for(std::size_t i = 0; i < m_size; ++i) - { - element_type element{m_buffer[i].getWidth(), - m_buffer[i].getHeight(), - m_buffer[i].getData() + - at * m_buffer[i].getStride(TensorDimension::DEPTH), - m_buffer[i].isCPU()}; - result[i] = std::move(element); - } - } - - return result; - } - - template::type> - frame_type getFrame(const key_type & idx, ChannelCount UNUSED = T) - { - frame_type result; - - if(m_mapping.find(idx) != m_mapping.end()) - { - std::size_t at = m_mapping[idx]; - result = std::move(frame_type{m_buffer.getSize(), m_buffer.isCPU()}); - result.setSize(m_size); - for(std::size_t i = 0; i < m_size; ++i) - { - element_type element{m_buffer[i].getWidth(), - m_buffer[i].getHeight(), - m_buffer[i].getChannelCount(), - m_buffer[i].getData() + - at * m_buffer[i].getStride(TensorDimension::DEPTH), - m_buffer[i].isCPU()}; - result[i] = std::move(element); - } - } - - return result; - } - - template::type> - frame_type getFrame(key_type && idx) - { - frame_type result; - - if(m_mapping.find(idx) != m_mapping.end()) - { - std::size_t at = m_mapping[idx]; - result = std::move(frame_type{m_buffer.getSize(), m_buffer.isCPU()}); - result.setSize(m_size); - for(std::size_t i = 0; i < m_size; ++i) - { - element_type element{m_buffer[i].getWidth(), - m_buffer[i].getHeight(), - m_buffer[i].getData() + - at * m_buffer[i].getStride(TensorDimension::DEPTH), - m_buffer[i].isCPU()}; - result[i] = std::move(element); - } - } - - return result; - } - - template::type> - frame_type getFrame(key_type && idx, ChannelCount UNUSED = T) - { - frame_type result; - - if(m_mapping.find(idx) != m_mapping.end()) - { - std::size_t at = m_mapping[idx]; - result = std::move(frame_type{m_buffer.getSize(), m_buffer.isCPU()}); - result.setSize(m_size); - for(std::size_t i = 0; i < m_size; ++i) - { - element_type element{m_buffer[i].getWidth(), - m_buffer[i].getHeight(), - m_buffer[i].getChannelCount(), - m_buffer[i].getData() + - at * m_buffer[i].getStride(TensorDimension::DEPTH), - m_buffer[i].isCPU()}; - result[i] = std::move(element); - } - } - - return result; - } - - template::type> - element_type getElement(const key_type & keyIdx, std::size_t unitIdx) - { - element_type element; - - if(m_mapping.find(keyIdx) != m_mapping.end()) - { - std::size_t at = m_mapping[keyIdx]; - element = std::move(element_type{m_buffer[unitIdx].getWidth(), - m_buffer[unitIdx].getHeight(), - m_buffer[unitIdx].getData() + - at * m_buffer[unitIdx].getStride(TensorDimension::DEPTH), - m_buffer[unitIdx].isCPU()}); - } - - return element; - } - - template::type> - element_type getElement(const key_type & keyIdx, std::size_t unitIdx, ChannelCount UNUSED = T) - { - element_type element; - - if(m_mapping.find(keyIdx) != m_mapping.end()) - { - std::size_t at = m_mapping[keyIdx]; - element = std::move(element_type{m_buffer[unitIdx].getWidth(), - m_buffer[unitIdx].getHeight(), - m_buffer[unitIdx].getChannelCount(), - m_buffer[unitIdx].getData() + - at * m_buffer[unitIdx].getStride(TensorDimension::DEPTH), - m_buffer[unitIdx].isCPU()}); - } - - return element; - } - - template::type> - element_type getElement(key_type && keyIdx, std::size_t unitIdx) - { - element_type element; - - if(m_mapping.find(keyIdx) != m_mapping.end()) - { - std::size_t at = m_mapping[keyIdx]; - element = std::move(element_type{m_buffer[unitIdx].getWidth(), - m_buffer[unitIdx].getHeight(), - m_buffer[unitIdx].getData() + - at * m_buffer[unitIdx].getStride(TensorDimension::DEPTH), - m_buffer[unitIdx].isCPU()}); - } - - return element; - } - - template::type> - element_type getElement(key_type && keyIdx, std::size_t unitIdx, ChannelCount UNUSED = T) - { - element_type element; - - if(m_mapping.find(keyIdx) != m_mapping.end()) - { - std::size_t at = m_mapping[keyIdx]; - element = std::move(element_type{m_buffer[unitIdx].getWidth(), - m_buffer[unitIdx].getHeight(), - m_buffer[unitIdx].getChannelCount(), - m_buffer[unitIdx].getData() + - at * m_buffer[unitIdx].getStride(TensorDimension::DEPTH), - m_buffer[unitIdx].isCPU()}); - } - - return element; - } - - /** - * Get the ChannelType of the Tensor. - * @return ChannelType of the Tensor. - */ - constexpr ChannelType getType() const noexcept - { - return CT; - } - - /** - * Get the flag whether the Tensor is allocated in CPU or GPU. - * @return whether the Tensor is allocated in CPU. - */ - bool isCPU() const noexcept - { - return m_isCPU; - } - - private: - // Mapping and Pool form a unique-to-unique isometry between - // the keys and indices of the batch dimension - mutable std::unordered_map m_mapping; - mutable std::set m_pool; - - std::size_t m_maxBatchSize; - std::size_t m_size; - bool m_isCPU; - - buffer_type m_buffer; -}; - -} // namespace cvcore - -#endif // CVCORE_TENSORMAP_H diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/ITensorOperatorStream.h b/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/ITensorOperatorStream.h deleted file mode 100644 index c4de9df..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/ITensorOperatorStream.h +++ /dev/null @@ -1,251 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 - -#ifndef CVCORE_ITENSOROPERATORSTREAM_H -#define CVCORE_ITENSOROPERATORSTREAM_H - -#include -#include -#include -#include - -#include "cv/core/CameraModel.h" -#include "cv/core/ComputeEngine.h" -#include "cv/core/Image.h" - -#include "Errors.h" -#include "IImageWarp.h" -#include "ImageUtils.h" -namespace cvcore { namespace tensor_ops { - -class NotImplementedException : public std::logic_error -{ -public: - NotImplementedException() - : std::logic_error("Method not yet implemented.") - { - } -}; - -class ITensorOperatorStream -{ -public: - // Public Constructor(s)/Destructor - virtual ~ITensorOperatorStream() noexcept = default; - - // Public Accessor Method(s) - virtual std::error_code Status() noexcept = 0; - - virtual std::error_code GenerateWarpFromCameraModel(ImageWarp & warp, - const ImageGrid & grid, - const CameraModel & source, - const CameraIntrinsics & target) = 0; - - virtual std::error_code DestroyWarp(ImageWarp & warp) noexcept = 0; - - // Public Mutator Method(s) - virtual std::error_code Remap(Image &outputImage, const Image &inputImage, - const ImageWarp warp, - InterpolationType interpolation = INTERP_LINEAR, - BorderType border = BORDER_ZERO) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - virtual std::error_code Remap(Image &outputImage, const Image &inputImage, - const ImageWarp warp, - InterpolationType interpolation = INTERP_LINEAR, - BorderType border = BORDER_ZERO) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - - virtual std::error_code Remap(Image &outputImage, const Image &inputImage, - const ImageWarp warp, - InterpolationType interpolation = INTERP_LINEAR, - BorderType border = BORDER_ZERO) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - - virtual std::error_code Remap(Image &outputImage, const Image &inputImage, - const ImageWarp warp, - InterpolationType interpolation = INTERP_LINEAR, - BorderType border = BORDER_ZERO) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - - virtual std::error_code Resize(Image &outputImage, const Image &inputImage, - InterpolationType interpolation = INTERP_LINEAR, BorderType border = BORDER_ZERO) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - virtual std::error_code Resize(Image &outputImage, const Image &inputImage, - InterpolationType interpolation = INTERP_LINEAR, BorderType border = BORDER_ZERO) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - virtual std::error_code Resize(Image &outputImage, const Image &inputImage, - InterpolationType interpolation = INTERP_LINEAR, BorderType border = BORDER_ZERO) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - virtual std::error_code Resize(Image &outputImage, const Image &inputImage, - InterpolationType interpolation = INTERP_LINEAR, BorderType border = BORDER_ZERO) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - virtual std::error_code Resize(Image &outputImage, const Image &inputImage, - InterpolationType interpolation = INTERP_LINEAR, BorderType border = BORDER_ZERO) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - - virtual std::error_code Normalize(Image &outputImage, const Image &inputImage - /* only configuration parameters */) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - virtual std::error_code Normalize(Image &outputImage, const Image &inputImage - /* only configuration parameters */) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - virtual std::error_code Normalize(Image &outputImage, const Image &inputImage - /* only configuration parameters */) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - virtual std::error_code Normalize(Image &outputImage, const Image &inputImage - /* only configuration parameters */) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - - virtual std::error_code ColorConvert(Image &outputImage, const Image &inputImage) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - - virtual std::error_code ColorConvert(Image &outputImage, const Image &inputImage) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - - virtual std::error_code ColorConvert(Image &outputImage, const Image &inputImage) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - - virtual std::error_code ColorConvert(Image &outputImage, const Image &inputImage) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - - virtual std::error_code ColorConvert(Image &outputImage, const Image &inputImage) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - - virtual std::error_code ColorConvert(Image &outputImage, const Image &inputImage) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - - virtual std::error_code ColorConvert(Image &outputImage, const Image &inputImage) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - - virtual std::error_code ColorConvert(Image &outputImage, const Image &inputImage) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - - virtual std::error_code ColorConvert(Image &outputImage, const Image &inputImage) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - - virtual std::error_code ColorConvert(Image &outputImage, const Image &inputImage) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - - virtual std::error_code StereoDisparityEstimator(Image &outputImage, const Image &inputLeftImage, - const Image &inputRightImage, size_t windowSize, - size_t maxDisparity) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - - virtual std::error_code StereoDisparityEstimator(Image &outputImage, const Image &inputLeftImage, - const Image &inputRightImage, size_t windowSize, - size_t maxDisparity) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - - virtual std::error_code StereoDisparityEstimator(Image &outputImage, const Image &inputLeftImage, - const Image &inputRightImage, size_t windowSize, - size_t maxDisparity) - { - throw NotImplementedException(); - return make_error_code(ErrorCode::NOT_IMPLEMENTED); - } - -protected: - // Protected Constructor(s) - ITensorOperatorStream() = default; - ITensorOperatorStream(const ITensorOperatorStream &) = default; - ITensorOperatorStream(ITensorOperatorStream &&) noexcept = default; - - // Protected Operator(s) - ITensorOperatorStream &operator=(const ITensorOperatorStream &) = default; - ITensorOperatorStream &operator=(ITensorOperatorStream &&) noexcept = default; -}; - -using TensorOperatorStream = ITensorOperatorStream *; - -}} // namespace cvcore::tensor_ops - -#endif // CVCORE_ITENSOROPERATORSTREAM_H diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/core/cvcore/Dummy.cu b/isaac_ros_image_proc/gxf/tensorops/cvcore/src/core/cvcore/Dummy.cu deleted file mode 100644 index e69de29..0000000 diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/core/utility/CVError.cpp b/isaac_ros_image_proc/gxf/tensorops/cvcore/src/core/utility/CVError.cpp deleted file mode 100644 index a6e62c7..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/core/utility/CVError.cpp +++ /dev/null @@ -1,123 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 - -#include "cv/core/CVError.h" - -#include -#include -#include - -#ifndef __cpp_lib_to_underlying -// Using a C++23 feature by hacking std -namespace std -{ - template - constexpr underlying_type_t to_underlying(Enum e) noexcept - { - return static_cast>(e); - } -}; -#endif // __cpp_lib_to_underlying - -namespace cvcore { - -namespace detail -{ - struct CoreErrorCategory : std::error_category - { - virtual const char * name() const noexcept override final - { - return "cvcore-error"; - } - - virtual std::string message(int value) const override final - { - std::string result; - - switch(value) - { - case std::to_underlying(ErrorCode::SUCCESS): - result = "(SUCCESS) No errors detected"; - break; - case std::to_underlying(ErrorCode::NOT_READY): - result = "(NOT_READY) The execution of the requested " - "operation is not to return"; - break; - case std::to_underlying(ErrorCode::NOT_IMPLEMENTED): - result = "(NOT_IMPLEMENTED) The requested operation is not " - "implemented"; - break; - case std::to_underlying(ErrorCode::INVALID_ARGUMENT): - result = "(INVALID_ARGUMENT) The argument provided to the " - "operation is not currently supported"; - break; - case std::to_underlying(ErrorCode::INVALID_IMAGE_FORMAT): - result = "(INVALID_IMAGE_FORMAT) The requested image format " - "is not supported by the operation"; - break; - case std::to_underlying(ErrorCode::INVALID_STORAGE_TYPE): - result = "(INVALID_STORAGE_TYPE) The requested storage type " - "is not supported by the operation"; - break; - case std::to_underlying(ErrorCode::INVALID_ENGINE_TYPE): - result = "(INVALID_ENGINE_TYPE) The requested engine type " - "is not supported by the operation"; - break; - case std::to_underlying(ErrorCode::INVALID_OPERATION): - result = "(INVALID_OPERATION) The requested operation is " - "not supported"; - break; - case std::to_underlying(ErrorCode::DETECTED_NAN_IN_RESULT): - result = "(DETECTED_NAN_IN_RESULT) NaN was detected in the " - "return value of the operation"; - break; - case std::to_underlying(ErrorCode::OUT_OF_MEMORY): - result = "(OUT_OF_MEMORY) The device has run out of memory"; - break; - case std::to_underlying(ErrorCode::DEVICE_ERROR): - result = "(DEVICE_ERROR) A device level error has been " - "encountered"; - break; - case std::to_underlying(ErrorCode::SYSTEM_ERROR): - result = "(SYSTEM_ERROR) A system level error has been " - "encountered"; - break; - default: - result = "(Unrecognized Condition) Value " + std::to_string(value) + - " does not map to known error code literal " + - " defined by cvcore::ErrorCode"; - break; - } - - return result; - } - }; -} // namespace detail - -const detail::CoreErrorCategory errorCategory{}; - -std::error_condition make_error_condition(ErrorCode ec) noexcept -{ - return {std::to_underlying(ec), errorCategory}; -} - -std::error_code make_error_code(ErrorCode ec) noexcept -{ - return {std::to_underlying(ec), errorCategory}; -} - -} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/core/utility/Instrumentation.cpp b/isaac_ros_image_proc/gxf/tensorops/cvcore/src/core/utility/Instrumentation.cpp deleted file mode 100644 index 583b646..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/core/utility/Instrumentation.cpp +++ /dev/null @@ -1,95 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 - -#include "cv/core/Instrumentation.h" - -#ifdef NVBENCH_ENABLE -#include -#include -#include -#endif - -namespace cvcore { namespace profiler { - -#ifdef NVBENCH_ENABLE -nv::bench::JsonHelper mapProfilerJsonOutputTypeToNvbenchType(ProfilerJsonOutputType jsonType) -{ - nv::bench::JsonHelper nvbenchJsonOutputType = nv::bench::JsonHelper::JSON_OFF; - if (jsonType == ProfilerJsonOutputType::JSON_OFF) - { - nvbenchJsonOutputType = nv::bench::JsonHelper::JSON_OFF; - } - else if (jsonType == ProfilerJsonOutputType::JSON_SEPARATE) - { - nvbenchJsonOutputType = nv::bench::JsonHelper::JSON_SEPARATE; - } - else if (jsonType == ProfilerJsonOutputType::JSON_AGGREGATE) - { - nvbenchJsonOutputType = nv::bench::JsonHelper::JSON_AGGREGATE; - } - return nvbenchJsonOutputType; -} -#endif - -void flush(const std::string& filename, ProfilerJsonOutputType jsonType) -{ -#ifdef NVBENCH_ENABLE - nv::bench::JsonHelper nvbenchJsonOutputType = mapProfilerJsonOutputTypeToNvbenchType(jsonType); - if (!filename.empty()) - { - nv::bench::Pool::instance().flushToFile(filename.c_str(), -1, INT_MAX, nvbenchJsonOutputType); - } - else - { - nv::bench::Pool::instance().flush(std::clog, -1, INT_MAX, nvbenchJsonOutputType); - } -#else - return; -#endif - -} - -void flush(std::ostream& output, ProfilerJsonOutputType jsonType) -{ -#ifdef NVBENCH_ENABLE - nv::bench::JsonHelper nvbenchJsonOutputType = mapProfilerJsonOutputTypeToNvbenchType(jsonType); - nv::bench::Pool::instance().flush(output, -1, INT_MAX, nvbenchJsonOutputType); -#else - return; -#endif -} - -void flush(ProfilerJsonOutputType jsonType) -{ -#ifdef NVBENCH_ENABLE - nv::bench::JsonHelper nvbenchJsonOutputType = mapProfilerJsonOutputTypeToNvbenchType(jsonType); - nv::bench::Pool::instance().flush(std::clog, -1, INT_MAX, nvbenchJsonOutputType); -#else - return; -#endif -} - -void clear() -{ -#ifdef NVBENCH_ENABLE - nv::bench::Pool::instance().clear(); -#else - return; -#endif -} - -}} diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/core/utility/ProfileUtils.cpp b/isaac_ros_image_proc/gxf/tensorops/cvcore/src/core/utility/ProfileUtils.cpp deleted file mode 100644 index 233ae19..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/core/utility/ProfileUtils.cpp +++ /dev/null @@ -1,127 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2020-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 - -#include - -#include - -#include -#include -#include - -#if defined(_MSC_VER) || defined(__WIN32) -# include -# include -# include -#endif - -using json = nlohmann::json; - -namespace cvcore { - -namespace { - -#if defined(_MSC_VER) || defined(__WIN32) -std::string GetCPUName() -{ - // https://docs.microsoft.com/en-us/previous-versions/visualstudio/visual-studio-2008/hskdteyh(v=vs.90)?redirectedfrom=MSDN - char CPUBrandString[0x40]; - int CPUInfo[4] = {-1}; - - // Calling __cpuid with 0x80000000 as the InfoType argument - // gets the number of valid extended IDs. - __cpuid(CPUInfo, 0x80000000); - unsigned i, nExIds = CPUInfo[0]; - memset(CPUBrandString, 0, sizeof(CPUBrandString)); - - // Get the information associated with each extended ID. - for (i=0x80000000; i<=nExIds; ++i) - { - __cpuid(CPUInfo, i); - - // Interpret CPU brand string and cache information. - if (i == 0x80000002) - memcpy(CPUBrandString, CPUInfo, sizeof(CPUInfo)); - else if (i == 0x80000003) - memcpy(CPUBrandString + 16, CPUInfo, sizeof(CPUInfo)); - else if (i == 0x80000004) - memcpy(CPUBrandString + 32, CPUInfo, sizeof(CPUInfo)); - } - return CPUBrandString; -} -#else -std::string GetCPUName() -{ - std::ifstream cpuInfo("/proc/cpuinfo"); - if (!cpuInfo.good()) - { - throw std::runtime_error("unable to retrieve cpu info"); - } - std::string line; - while (std::getline(cpuInfo, line)) - { - int delimiterPos = line.find(':'); - if (delimiterPos != std::string::npos) - { - std::string key = line.substr(0, delimiterPos); - if (key.find("model name") != std::string::npos) - { - std::string info = line.substr(delimiterPos + 1); - info.erase(0, info.find_first_not_of(' ')); - return info; - } - } - } - return "CPU"; // default name if no cpu model name retrieved -} -#endif - -std::string GetGPUName() -{ - int deviceId; - cudaGetDevice(&deviceId); - cudaDeviceProp prop; - cudaError_t error = cudaGetDeviceProperties(&prop, deviceId); - if (error != 0) - { - throw std::runtime_error("unable to retrieve cuda device info"); - } - return std::string(prop.name); -} - -} // anonymous namespace - -void ExportToJson(const std::string outputPath, const std::string taskName, float tMin, float tMax, float tAvg, - bool isCPU, int iterations = 100) -{ - std::ifstream in(outputPath); - json jsonHandler; - if (in.good()) - { - in >> jsonHandler; - } - in.close(); - - const std::string platform = isCPU ? "CPU: " + GetCPUName() : "GPU: " + GetGPUName(); - jsonHandler[platform][taskName] = {{"iter", iterations}, {"min", tMin}, {"max", tMax}, {"avg", tAvg}}; - - std::ofstream out(outputPath); - out << std::setw(4) << jsonHandler << std::endl; - out.close(); -} - -} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/ArithmeticOperations.cpp b/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/ArithmeticOperations.cpp deleted file mode 100644 index 85d5e2f..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/ArithmeticOperations.cpp +++ /dev/null @@ -1,329 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2020-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 - -#include "NppUtils.h" - -#include "cv/tensor_ops/ImageUtils.h" - -#include "cv/core/Memory.h" - -#include -#include - -#include -#include -#include - -namespace cvcore { namespace tensor_ops { - -namespace { - -static void NormalizeTensorC3F32Inplace(Tensor &src, const float scale[3], const float offset[3], - NppStreamContext streamContext) -{ - const int srcW = src.getWidth(); - const int srcH = src.getHeight(); - const NppiSize srcSize = {srcW, srcH}; - - const Npp32f offsets[3] = {static_cast(offset[0]), static_cast(offset[1]), - static_cast(offset[2])}; - NppStatus status = - nppiAddC_32f_C3IR_Ctx(offsets, static_cast(src.getData()), - src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), srcSize, streamContext); - assert(status == NPP_SUCCESS); - - const Npp32f scales[3] = {static_cast(scale[0]), static_cast(scale[1]), - static_cast(scale[2])}; - status = nppiMulC_32f_C3IR_Ctx(scales, static_cast(src.getData()), - src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), srcSize, streamContext); - assert(status == NPP_SUCCESS); -} - -template -static void NormalizeTensorC1F32Inplace(Tensor &src, const float scale, const float offset, - NppStreamContext streamContext) -{ - const int srcW = src.getWidth(); - const int srcH = src.getHeight(); - const NppiSize srcSize = {srcW, srcH}; - - NppStatus status = - nppiAddC_32f_C1IR_Ctx(static_cast(offset), static_cast(src.getData()), - src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), srcSize, streamContext); - assert(status == NPP_SUCCESS); - - status = nppiMulC_32f_C1IR_Ctx(static_cast(scale), static_cast(src.getData()), - src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), srcSize, streamContext); - assert(status == NPP_SUCCESS); -} - -template -void NormalizeC1U8Impl(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert((src.getWidth() == dst.getWidth()) && (src.getHeight() == dst.getHeight())); - - NppStreamContext streamContext = GetNppStreamContext(stream); - - NppStatus status = nppiConvert_8u32f_C1R_Ctx( - static_cast(src.getData()), src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), - {int(src.getWidth()), int(src.getHeight())}, streamContext); - assert(status == NPP_SUCCESS); - - NormalizeTensorC1F32Inplace(dst, scale, offset, streamContext); -} - -template -void NormalizeC1U16Impl(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert((src.getWidth() == dst.getWidth()) && (src.getHeight() == dst.getHeight())); - - NppStreamContext streamContext = GetNppStreamContext(stream); - - NppStatus status = nppiConvert_16u32f_C1R_Ctx( - static_cast(src.getData()), src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), - {int(src.getWidth()), int(src.getHeight())}, streamContext); - assert(status == NPP_SUCCESS); - - NormalizeTensorC1F32Inplace(dst, scale, offset, streamContext); -} - -template -void NormalizeC1F32Impl(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert((src.getWidth() == dst.getWidth()) && (src.getHeight() == dst.getHeight())); - - Copy(dst, src, stream); - NormalizeTensorC1F32Inplace(dst, scale, offset, GetNppStreamContext(stream)); -} - -template -void NormalizeC3Batch(Tensor &dst, Tensor &src, const float scale[3], - const float offset[3], cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert(src.getDepth() == dst.getDepth()); - - for (int i = 0; i < src.getDepth(); i++) - { - size_t shiftSrc = i * src.getStride(TensorDimension::DEPTH); - size_t shiftDst = i * dst.getStride(TensorDimension::DEPTH); - Tensor srcTmp(src.getWidth(), src.getHeight(), - src.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), - src.getData() + shiftSrc, false); - Tensor dstTmp(dst.getWidth(), dst.getHeight(), - dst.getStride(TensorDimension::HEIGHT) * GetChannelSize(F32), - dst.getData() + shiftDst, false); - Normalize(dstTmp, srcTmp, scale, offset, stream); - } -} - -template -void NormalizeC1Batch(Tensor &dst, Tensor &src, const float scale, const float offset, - cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert(src.getDepth() == dst.getDepth()); - - for (int i = 0; i < src.getDepth(); i++) - { - size_t shiftSrc = i * src.getStride(TensorDimension::DEPTH); - size_t shiftDst = i * dst.getStride(TensorDimension::DEPTH); - Tensor srcTmp(src.getWidth(), src.getHeight(), - src.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), - src.getData() + shiftSrc, false); - Tensor dstTmp(dst.getWidth(), dst.getHeight(), - dst.getStride(TensorDimension::HEIGHT) * GetChannelSize(F32), - dst.getData() + shiftDst, false); - Normalize(dstTmp, srcTmp, scale, offset, stream); - } -} - -template -void NormalizeC1Batch(Tensor &dst, Tensor &src, const float scale, const float offset, - cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert(src.getDepth() == dst.getDepth()); - - for (int i = 0; i < src.getDepth(); i++) - { - size_t shiftSrc = i * src.getStride(TensorDimension::DEPTH); - size_t shiftDst = i * dst.getStride(TensorDimension::DEPTH); - Tensor srcTmp(src.getWidth(), src.getHeight(), - src.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), - src.getData() + shiftSrc, false); - Tensor dstTmp(dst.getWidth(), dst.getHeight(), - dst.getStride(TensorDimension::HEIGHT) * GetChannelSize(F32), - dst.getData() + shiftDst, false); - Normalize(dstTmp, srcTmp, scale, offset, stream); - } -} - -} // anonymous namespace - -void Normalize(Tensor &dst, const Tensor &src, const float scale[3], const float offset[3], - cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert((src.getWidth() == dst.getWidth()) && (src.getHeight() == dst.getHeight())); - - NppStreamContext streamContext = GetNppStreamContext(stream); - - NppStatus status = nppiConvert_8u32f_C3R_Ctx( - static_cast(src.getData()), src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), - {int(src.getWidth()), int(src.getHeight())}, streamContext); - assert(status == NPP_SUCCESS); - - NormalizeTensorC3F32Inplace(dst, scale, offset, streamContext); -} - -void Normalize(Tensor &dst, const Tensor &src, const float scale[3], const float offset[3], - cudaStream_t stream) -{ - NormalizeC3Batch(dst, const_cast &>(src), scale, offset, stream); -} - -void Normalize(Tensor &dst, const Tensor &src, const float scale[3], const float offset[3], - cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert((src.getWidth() == dst.getWidth()) && (src.getHeight() == dst.getHeight())); - - NppStreamContext streamContext = GetNppStreamContext(stream); - - NppStatus status = nppiConvert_16u32f_C3R_Ctx( - static_cast(src.getData()), src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), - {int(src.getWidth()), int(src.getHeight())}, streamContext); - assert(status == NPP_SUCCESS); - - NormalizeTensorC3F32Inplace(dst, scale, offset, streamContext); -} - -void Normalize(Tensor &dst, const Tensor &src, const float scale[3], - const float offset[3], cudaStream_t stream) -{ - NormalizeC3Batch(dst, const_cast &>(src), scale, offset, stream); -} - -void Normalize(Tensor &dst, const Tensor &src, const float scale[3], const float offset[3], - cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert((src.getWidth() == dst.getWidth()) && (src.getHeight() == dst.getHeight())); - - Copy(dst, src, stream); - NormalizeTensorC3F32Inplace(dst, scale, offset, GetNppStreamContext(stream)); -} - -void Normalize(Tensor &dst, const Tensor &src, const float scale[3], - const float offset[3], cudaStream_t stream) -{ - NormalizeC3Batch(dst, const_cast &>(src), scale, offset, stream); -} - -void Normalize(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream) -{ - NormalizeC1U8Impl(dst, src, scale, offset, stream); -} - -void Normalize(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream) -{ - NormalizeC1Batch(dst, const_cast &>(src), scale, offset, stream); -} - -void Normalize(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream) -{ - NormalizeC1U16Impl(dst, src, scale, offset, stream); -} - -void Normalize(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream) -{ - NormalizeC1Batch(dst, const_cast &>(src), scale, offset, stream); -} - -void Normalize(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream) -{ - NormalizeC1F32Impl(dst, src, scale, offset, stream); -} - -void Normalize(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream) -{ - NormalizeC1Batch(dst, const_cast &>(src), scale, offset, stream); -} - -void Normalize(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream) -{ - NormalizeC1U8Impl(dst, src, scale, offset, stream); -} - -void Normalize(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream) -{ - NormalizeC1Batch(dst, const_cast &>(src), scale, offset, stream); -} - -void Normalize(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream) -{ - NormalizeC1U16Impl(dst, src, scale, offset, stream); -} - -void Normalize(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream) -{ - NormalizeC1Batch(dst, const_cast &>(src), scale, offset, stream); -} - -void Normalize(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream) -{ - NormalizeC1F32Impl(dst, src, scale, offset, stream); -} - -void Normalize(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream) -{ - NormalizeC1Batch(dst, const_cast &>(src), scale, offset, stream); -} - -}} // namespace cvcore::tensor_ops \ No newline at end of file diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/ColorConversions.cpp b/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/ColorConversions.cpp deleted file mode 100644 index f02d3a9..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/ColorConversions.cpp +++ /dev/null @@ -1,447 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2020-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 - -#include "NppUtils.h" - -#include "cv/tensor_ops/ImageUtils.h" - -#include "cv/core/Memory.h" - -#include -#include -#include - -#include -#include -#include - -namespace cvcore { namespace tensor_ops { - -const float BGR2GRAY_COEFFS[3] = {0.114f, 0.587f, 0.299f}; -const float RGB2GRAY_COEFFS[3] = {0.299f, 0.587f, 0.114f}; - -namespace { - -template -void ConvertColorFormatBatch(Tensor &dst, Tensor &src, ColorConversionType type, - cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert(src.getDepth() == dst.getDepth()); - - for (int i = 0; i < src.getDepth(); i++) - { - size_t offsetSrc = i * src.getStride(TensorDimension::DEPTH); - size_t offsetDst = i * dst.getStride(TensorDimension::DEPTH); - Tensor srcTmp(src.getWidth(), src.getHeight(), - src.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), - src.getData() + offsetSrc, false); - Tensor dstTmp(dst.getWidth(), dst.getHeight(), - dst.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), - dst.getData() + offsetDst, false); - ConvertColorFormat(dstTmp, srcTmp, type, stream); - } -} - -template -void InterleavedToPlanarBatch(Tensor &dst, Tensor &src, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert(src.getDepth() == dst.getDepth()); - - for (int i = 0; i < src.getDepth(); i++) - { - size_t offsetSrc = i * src.getStride(TensorDimension::DEPTH); - size_t offsetDst = i * dst.getStride(TensorDimension::DEPTH); - Tensor srcTmp(src.getWidth(), src.getHeight(), - src.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), - src.getData() + offsetSrc, false); - Tensor dstTmp(dst.getWidth(), dst.getHeight(), - dst.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), - dst.getData() + offsetDst, false); - InterleavedToPlanar(dstTmp, srcTmp, stream); - } -} - -} // anonymous namespace - -void ConvertColorFormat(Tensor &dst, const Tensor &src, ColorConversionType type, - cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert((src.getWidth() == dst.getWidth()) && (src.getHeight() == dst.getHeight())); - - if (type == BGR2RGB || type == RGB2BGR) - { - const int order[3] = {2, 1, 0}; - NppStatus status = nppiSwapChannels_8u_C3R_Ctx( - static_cast(src.getData()), src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), - {int(src.getWidth()), int(src.getHeight())}, order, GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); - } - else - { - throw std::runtime_error("invalid color conversion type"); - } -} - -void ConvertColorFormat(Tensor &dst, const Tensor &src, ColorConversionType type, - cudaStream_t stream) -{ - ConvertColorFormatBatch(dst, const_cast &>(src), type, stream); -} - -void ConvertColorFormat(Tensor &dst, const Tensor &src, ColorConversionType type, - cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert((src.getWidth() == dst.getWidth()) && (src.getHeight() == dst.getHeight())); - - if (type == BGR2RGB || type == RGB2BGR) - { - const int order[3] = {2, 1, 0}; - NppStatus status = nppiSwapChannels_16u_C3R_Ctx( - static_cast(src.getData()), src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), - {int(src.getWidth()), int(src.getHeight())}, order, GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); - } - else - { - throw std::runtime_error("invalid color conversion type"); - } -} - -void ConvertColorFormat(Tensor &dst, const Tensor &src, ColorConversionType type, - cudaStream_t stream) -{ - ConvertColorFormatBatch(dst, const_cast &>(src), type, stream); -} - -void ConvertColorFormat(Tensor &dst, const Tensor &src, ColorConversionType type, - cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert((src.getWidth() == dst.getWidth()) && (src.getHeight() == dst.getHeight())); - - if (type == BGR2RGB || type == RGB2BGR) - { - const int order[3] = {2, 1, 0}; - NppStatus status = nppiSwapChannels_32f_C3R_Ctx( - static_cast(src.getData()), src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), - {int(src.getWidth()), int(src.getHeight())}, order, GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); - } - else - { - throw std::runtime_error("invalid color conversion type"); - } -} - -void ConvertColorFormat(Tensor &dst, const Tensor &src, ColorConversionType type, - cudaStream_t stream) -{ - ConvertColorFormatBatch(dst, const_cast &>(src), type, stream); -} - -void ConvertColorFormat(Tensor &dst, const Tensor &src, ColorConversionType type, - cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert((src.getWidth() == dst.getWidth()) && (src.getHeight() == dst.getHeight())); - - if (type == BGR2GRAY || type == RGB2GRAY) - { - NppStatus status = nppiColorToGray_8u_C3C1R_Ctx( - static_cast(src.getData()), src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), - {int(src.getWidth()), int(src.getHeight())}, type == BGR2GRAY ? BGR2GRAY_COEFFS : RGB2GRAY_COEFFS, - GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); - } - else - { - throw std::runtime_error("invalid color conversion type"); - } -} - -void ConvertColorFormat(Tensor &dst, const Tensor &src, ColorConversionType type, - cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert((src.getWidth() == dst.getWidth()) && (src.getHeight() == dst.getHeight())); - - if (type == BGR2GRAY || type == RGB2GRAY) - { - NppStatus status = nppiColorToGray_16u_C3C1R_Ctx( - static_cast(src.getData()), src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), - {int(src.getWidth()), int(src.getHeight())}, type == BGR2GRAY ? BGR2GRAY_COEFFS : RGB2GRAY_COEFFS, - GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); - } - else - { - throw std::runtime_error("invalid color conversion type"); - } -} - -void ConvertColorFormat(Tensor &dst, const Tensor &src, ColorConversionType type, - cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert((src.getWidth() == dst.getWidth()) && (src.getHeight() == dst.getHeight())); - - if (type == BGR2GRAY || type == RGB2GRAY) - { - NppStatus status = nppiColorToGray_32f_C3C1R_Ctx( - static_cast(src.getData()), src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), - {int(src.getWidth()), int(src.getHeight())}, type == BGR2GRAY ? BGR2GRAY_COEFFS : RGB2GRAY_COEFFS, - GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); - } - else - { - throw std::runtime_error("invalid color conversion type"); - } -} - -void ConvertColorFormat(Tensor &dst, const Tensor &src, ColorConversionType type, - cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert((src.getWidth() == dst.getWidth()) && (src.getHeight() == dst.getHeight())); - - if (type == GRAY2BGR || type == GRAY2RGB) - { - NppStatus status = nppiDup_8u_C1C3R_Ctx( - static_cast(src.getData()), src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), - {int(src.getWidth()), int(src.getHeight())}, GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); - } - else - { - throw std::runtime_error("invalid color conversion type"); - } -} - -void ConvertColorFormat(Tensor &dst, const Tensor &src, ColorConversionType type, - cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert((src.getWidth() == dst.getWidth()) && (src.getHeight() == dst.getHeight())); - - if (type == GRAY2BGR || type == GRAY2RGB) - { - NppStatus status = nppiDup_16u_C1C3R_Ctx( - static_cast(src.getData()), src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), - {int(src.getWidth()), int(src.getHeight())}, GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); - } - else - { - throw std::runtime_error("invalid color conversion type"); - } -} - -void ConvertColorFormat(Tensor &dst, const Tensor &src, ColorConversionType type, - cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert((src.getWidth() == dst.getWidth()) && (src.getHeight() == dst.getHeight())); - - if (type == GRAY2BGR || type == GRAY2RGB) - { - NppStatus status = nppiDup_32f_C1C3R_Ctx( - static_cast(src.getData()), src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), - {int(src.getWidth()), int(src.getHeight())}, GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); - } - else - { - throw std::runtime_error("invalid color conversion type"); - } -} - -void ConvertBitDepth(Tensor &dst, Tensor &src, const float scale, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert((src.getWidth() == dst.getWidth()) && (src.getHeight() == dst.getHeight())); - - const NppiSize srcSize = {src.getWidth(), src.getHeight()}; - - NppStreamContext streamContext = GetNppStreamContext(stream); - - NppStatus status = - nppiMulC_32f_C1IR_Ctx(static_cast(scale), static_cast(src.getData()), - src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), srcSize, streamContext); - assert(status == NPP_SUCCESS); - - status = nppiConvert_32f8u_C1R_Ctx( - static_cast(src.getData()), src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), - {int(src.getWidth()), int(src.getHeight())}, NPP_RND_FINANCIAL, streamContext); - assert(status == NPP_SUCCESS); -} - -void ConvertBitDepth(Tensor &dst, Tensor &src, const float scale, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert(src.getDepth() == dst.getDepth()); - - Tensor srcTmp(src.getWidth(), src.getDepth() * src.getHeight(), - src.getStride(TensorDimension::HEIGHT) * GetChannelSize(F32), src.getData(), false); - Tensor dstTmp(dst.getWidth(), dst.getDepth() * dst.getHeight(), - dst.getStride(TensorDimension::HEIGHT) * GetChannelSize(U8), dst.getData(), false); - ConvertBitDepth(dstTmp, srcTmp, scale, stream); -} - -void InterleavedToPlanar(Tensor &dst, const Tensor &src, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert((src.getWidth() == dst.getWidth()) && (src.getHeight() == dst.getHeight())); - - NppStatus status; - NppStreamContext streamContext = GetNppStreamContext(stream); - - const size_t offset = dst.getStride(TensorDimension::HEIGHT) * dst.getHeight(); - Npp8u *const dstBuffer[3] = {dst.getData(), dst.getData() + offset, dst.getData() + 2 * offset}; - status = nppiCopy_8u_C3P3R_Ctx(static_cast(src.getData()), - src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), dstBuffer, - dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), - {int(src.getWidth()), int(src.getHeight())}, streamContext); - assert(status == NPP_SUCCESS); -} - -void InterleavedToPlanar(Tensor &dst, const Tensor &src, cudaStream_t stream) -{ - InterleavedToPlanarBatch(dst, const_cast &>(src), stream); -} - -void InterleavedToPlanar(Tensor &dst, const Tensor &src, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert((src.getWidth() == dst.getWidth()) && (src.getHeight() == dst.getHeight())); - - NppStatus status; - NppStreamContext streamContext = GetNppStreamContext(stream); - - const size_t offset = dst.getStride(TensorDimension::HEIGHT) * dst.getHeight(); - Npp16u *const dstBuffer[3] = {dst.getData(), dst.getData() + offset, dst.getData() + 2 * offset}; - status = nppiCopy_16u_C3P3R_Ctx(static_cast(src.getData()), - src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), dstBuffer, - dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), - {int(src.getWidth()), int(src.getHeight())}, streamContext); -} - -void InterleavedToPlanar(Tensor &dst, const Tensor &src, cudaStream_t stream) -{ - InterleavedToPlanarBatch(dst, const_cast &>(src), stream); -} - -void InterleavedToPlanar(Tensor &dst, const Tensor &src, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert((src.getWidth() == dst.getWidth()) && (src.getHeight() == dst.getHeight())); - - NppStatus status; - NppStreamContext streamContext = GetNppStreamContext(stream); - - const size_t offset = dst.getStride(TensorDimension::HEIGHT) * dst.getHeight(); - Npp32f *const dstBuffer[3] = {dst.getData(), dst.getData() + offset, dst.getData() + 2 * offset}; - status = nppiCopy_32f_C3P3R_Ctx(static_cast(src.getData()), - src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), dstBuffer, - dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), - {int(src.getWidth()), int(src.getHeight())}, streamContext); -} - -void InterleavedToPlanar(Tensor &dst, const Tensor &src, cudaStream_t stream) -{ - InterleavedToPlanarBatch(dst, const_cast &>(src), stream); -} - -void InterleavedToPlanar(Tensor &dst, const Tensor &src, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert((src.getWidth() == dst.getWidth()) && (src.getHeight() == dst.getHeight())); - - Tensor tmp(dst.getWidth(), dst.getHeight(), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), - dst.getData(), false); - Copy(tmp, src, stream); -} - -void InterleavedToPlanar(Tensor &dst, const Tensor &src, cudaStream_t stream) -{ - InterleavedToPlanarBatch(dst, const_cast &>(src), stream); -} - -void InterleavedToPlanar(Tensor &dst, const Tensor &src, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert((src.getWidth() == dst.getWidth()) && (src.getHeight() == dst.getHeight())); - - Tensor tmp(dst.getWidth(), dst.getHeight(), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), - dst.getData(), false); - Copy(tmp, src, stream); -} - -void InterleavedToPlanar(Tensor &dst, const Tensor &src, cudaStream_t stream) -{ - InterleavedToPlanarBatch(dst, const_cast &>(src), stream); -} - -void InterleavedToPlanar(Tensor &dst, const Tensor &src, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert((src.getWidth() == dst.getWidth()) && (src.getHeight() == dst.getHeight())); - - Tensor tmp(dst.getWidth(), dst.getHeight(), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), - dst.getData(), false); - Copy(tmp, src, stream); -} - -void InterleavedToPlanar(Tensor &dst, const Tensor &src, cudaStream_t stream) -{ - InterleavedToPlanarBatch(dst, const_cast &>(src), stream); -} - -}} // namespace cvcore::tensor_ops diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/Errors.cpp b/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/Errors.cpp deleted file mode 100644 index d29a1ae..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/Errors.cpp +++ /dev/null @@ -1,104 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 - -#include "cv/tensor_ops/Errors.h" - -#ifndef __cpp_lib_to_underlying -// Using a C++23 feature by hacking std -namespace std -{ - template - constexpr underlying_type_t to_underlying(Enum e) noexcept - { - return static_cast>(e); - } -}; -#endif // __cpp_lib_to_underlying - -namespace cvcore { namespace tensor_ops { - -namespace detail -{ - struct TensorOpsErrorCategory : std::error_category - { - virtual const char * name() const noexcept override final - { - return "cvcore-tensor-ops-error"; - } - - virtual std::string message(int value) const override final - { - std::string result; - - switch(value) - { - case std::to_underlying(TensorOpsErrorCode::SUCCESS): - result = "(SUCCESS) No errors detected"; - break; - case std::to_underlying(TensorOpsErrorCode::COMPUTE_ENGINE_UNSUPPORTED_BY_CONTEXT): - result = "(COMPUTE_ENGINE_UNSUPPORTED_BY_CONTEXT) The selected compute " - "engine defined by cvcore::ComputeEngine is not avaible in the " - "requested context defined by cvcore::tensor_ops::TensorBackend"; - break; - case std::to_underlying(TensorOpsErrorCode::CAMERA_DISTORTION_MODEL_UNSUPPORTED): - result = "(CAMERA_DISTORTION_MODEL_UNSUPPORTED) The selected camera " - "distortion model defined by cvcore::CameraDistortionType is " - "currently unsupported"; - break; - default: - result = "(Unrecognized Condition) Value " + std::to_string(value) + - " does not map to known error code literal " + - " defined by cvcore::tensor_ops::TensorOpsErrorCode"; - break; - } - - return result; - } - - virtual std::error_condition default_error_condition(int code) const noexcept override final - { - std::error_condition result; - - switch(code) - { - case std::to_underlying(TensorOpsErrorCode::SUCCESS): - result = ErrorCode::SUCCESS; - break; - case std::to_underlying(TensorOpsErrorCode::COMPUTE_ENGINE_UNSUPPORTED_BY_CONTEXT): - result = ErrorCode::INVALID_ENGINE_TYPE; - break; - case std::to_underlying(TensorOpsErrorCode::CAMERA_DISTORTION_MODEL_UNSUPPORTED): - result = ErrorCode::INVALID_ARGUMENT; - break; - default: - result = ErrorCode::NOT_IMPLEMENTED; - break; - } - - return result; - } - }; -} // namespace detail - -const detail::TensorOpsErrorCategory errorCategory{}; - -std::error_code make_error_code(TensorOpsErrorCode ec) noexcept -{ - return {std::to_underlying(ec), errorCategory}; -} - -}} // namespace cvcore::tensor_ops diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/Filters.cpp b/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/Filters.cpp deleted file mode 100644 index d8bd75c..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/Filters.cpp +++ /dev/null @@ -1,112 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2020-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 - -#include "Filters.h" -#include "NppUtils.h" - -#include "cv/core/MathTypes.h" -#include "cv/core/Memory.h" - -#include - -#include -#include -#include - -namespace cvcore { namespace tensor_ops { - -void BoxFilter(Tensor &dst, const Tensor &src, const Vector2i &maskSize, - const Vector2i &anchor, cudaStream_t stream) -{ - assert(!src.isCPU() && !dst.isCPU()); - NppStatus status = nppiFilterBoxBorder_8u_C3R_Ctx( - static_cast(src.getData()), src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), - {static_cast(src.getWidth()), static_cast(src.getHeight())}, {0, 0}, - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), - {static_cast(src.getWidth()), static_cast(src.getHeight())}, {maskSize.x, maskSize.y}, - {anchor.x, anchor.y}, - NPP_BORDER_REPLICATE, //Only Npp Replicate is supported!!! - GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); -} - -void BoxFilter(Tensor &dst, const Tensor &src, const Vector2i &maskSize, - const Vector2i &anchor, cudaStream_t stream) -{ - assert(!src.isCPU() && !dst.isCPU()); - NppStatus status = nppiFilterBoxBorder_8u_C1R_Ctx( - static_cast(src.getData()), src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), - {static_cast(src.getWidth()), static_cast(src.getHeight())}, {0, 0}, - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), - {static_cast(src.getWidth()), static_cast(src.getHeight())}, {maskSize.x, maskSize.y}, - {anchor.x, anchor.y}, NPP_BORDER_REPLICATE, GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); -} - -void BoxFilter(Tensor &dst, const Tensor &src, const Vector2i &maskSize, - const Vector2i &anchor, cudaStream_t stream) -{ - assert(!src.isCPU() && !dst.isCPU()); - NppStatus status = nppiFilterBoxBorder_16u_C1R_Ctx( - static_cast(src.getData()), src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), - {static_cast(src.getWidth()), static_cast(src.getHeight())}, {0, 0}, - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), - {static_cast(src.getWidth()), static_cast(src.getHeight())}, {maskSize.x, maskSize.y}, - {anchor.x, anchor.y}, NPP_BORDER_REPLICATE, GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); -} - -void BoxFilter(Tensor &dst, const Tensor &src, const Vector2i &maskSize, - const Vector2i &anchor, cudaStream_t stream) -{ - assert(!src.isCPU() && !dst.isCPU()); - NppStatus status = nppiFilterBoxBorder_16u_C3R_Ctx( - static_cast(src.getData()), src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), - {static_cast(src.getWidth()), static_cast(src.getHeight())}, {0, 0}, - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), - {static_cast(src.getWidth()), static_cast(src.getHeight())}, {maskSize.x, maskSize.y}, - {anchor.x, anchor.y}, NPP_BORDER_REPLICATE, GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); -} - -void BoxFilter(Tensor &dst, const Tensor &src, const Vector2i &maskSize, - const Vector2i &anchor, cudaStream_t stream) -{ - assert(!src.isCPU() && !dst.isCPU()); - NppStatus status = nppiFilterBoxBorder_32f_C3R_Ctx( - static_cast(src.getData()), src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), - {static_cast(src.getWidth()), static_cast(src.getHeight())}, {0, 0}, - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), - {static_cast(src.getWidth()), static_cast(src.getHeight())}, {maskSize.x, maskSize.y}, - {anchor.x, anchor.y}, NPP_BORDER_REPLICATE, GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); -} - -void BoxFilter(Tensor &dst, const Tensor &src, const Vector2i &maskSize, - const Vector2i &anchor, cudaStream_t stream) -{ - assert(!src.isCPU() && !dst.isCPU()); - NppStatus status = nppiFilterBoxBorder_32f_C1R_Ctx( - static_cast(src.getData()), src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), - {static_cast(src.getWidth()), static_cast(src.getHeight())}, {0, 0}, - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), - {static_cast(src.getWidth()), static_cast(src.getHeight())}, {maskSize.x, maskSize.y}, - {anchor.x, anchor.y}, NPP_BORDER_REPLICATE, GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); -} - -}} // namespace cvcore::tensor_ops diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/GeometryTransforms.cpp b/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/GeometryTransforms.cpp deleted file mode 100644 index 238eac1..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/GeometryTransforms.cpp +++ /dev/null @@ -1,754 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2020-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 - -#include "NppUtils.h" - -#include "cv/tensor_ops/ImageUtils.h" - -#include "cv/core/Memory.h" - -#include -#include - -#include -#include -#include - -namespace cvcore { namespace tensor_ops { - -namespace { - -static NppiInterpolationMode GetNppiInterpolationMode(InterpolationType type) -{ - if (type == INTERP_NEAREST) - { - return NPPI_INTER_NN; - } - else if (type == INTERP_LINEAR) - { - return NPPI_INTER_LINEAR; - } - else if (type == INTERP_CUBIC_BSPLINE) - { - return NPPI_INTER_CUBIC2P_BSPLINE; - } - else if (type == INTERP_CUBIC_CATMULLROM) - { - return NPPI_INTER_CUBIC2P_CATMULLROM; - } - else - { - throw std::runtime_error("invalid resizing interpolation mode"); - } -} - -static BBox GetScaledROI(int srcW, int srcH, int dstW, int dstH) -{ - if (srcW * dstH >= dstW * srcH) - { - int bboxH = static_cast((static_cast(srcH) / srcW) * dstW); - int offsetH = (dstH - bboxH) / 2; - return {0, offsetH, dstW, offsetH + bboxH}; - } - else - { - int bboxW = static_cast((static_cast(srcW) / srcH) * dstH); - int offsetW = (dstW - bboxW) / 2; - return {offsetW, 0, offsetW + bboxW, dstH}; - } -} - -static void AssertValidROI(const BBox &roi, int width, int height) -{ - assert(roi.xmin >= 0 && roi.xmin < roi.xmax); - assert(roi.ymin >= 0 && roi.ymin < roi.ymax); - assert(roi.ymax <= height); - assert(roi.xmax <= width); -} - -template -void FillBufferC1U8Impl(Tensor &dst, const Npp8u value, cudaStream_t stream) -{ - assert(!dst.isCPU()); - NppStatus status = nppiSet_8u_C1R_Ctx(value, static_cast(dst.getData()), - dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), - {int(dst.getWidth()), int(dst.getHeight())}, GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); -} - -template -void FillBufferC1U16Impl(Tensor &dst, const Npp16u value, cudaStream_t stream) -{ - assert(!dst.isCPU()); - NppStatus status = nppiSet_16u_C1R_Ctx(value, static_cast(dst.getData()), - dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), - {int(dst.getWidth()), int(dst.getHeight())}, GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); -} - -template -void FillBufferC1F32Impl(Tensor &dst, const Npp32f value, cudaStream_t stream) -{ - assert(!dst.isCPU()); - NppStatus status = nppiSet_32f_C1R_Ctx(value, static_cast(dst.getData()), - dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), - {int(dst.getWidth()), int(dst.getHeight())}, GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); -} - -static void FillBuffer(Tensor &dst, const Npp8u value, cudaStream_t stream = 0) -{ - FillBufferC1U8Impl(dst, value, stream); -} - -static void FillBuffer(Tensor &dst, const Npp16u value, cudaStream_t stream = 0) -{ - FillBufferC1U16Impl(dst, value, stream); -} - -static void FillBuffer(Tensor &dst, const Npp32f value, cudaStream_t stream = 0) -{ - FillBufferC1F32Impl(dst, value, stream); -} - -static void FillBuffer(Tensor &dst, const Npp8u value, cudaStream_t stream = 0) -{ - FillBufferC1U8Impl(dst, value, stream); -} - -static void FillBuffer(Tensor &dst, const Npp16u value, cudaStream_t stream = 0) -{ - FillBufferC1U16Impl(dst, value, stream); -} - -static void FillBuffer(Tensor &dst, const Npp32f value, cudaStream_t stream = 0) -{ - FillBufferC1F32Impl(dst, value, stream); -} - -static void FillBuffer(Tensor &dst, const Npp8u value, cudaStream_t stream = 0) -{ - assert(!dst.isCPU()); - const Npp8u padding[3] = {value, value, value}; - NppStatus status = nppiSet_8u_C3R_Ctx(padding, static_cast(dst.getData()), - dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), - {int(dst.getWidth()), int(dst.getHeight())}, GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); -} - -static void FillBuffer(Tensor &dst, const Npp16u value, cudaStream_t stream = 0) -{ - assert(!dst.isCPU()); - const Npp16u padding[3] = {value, value, value}; - NppStatus status = nppiSet_16u_C3R_Ctx(padding, static_cast(dst.getData()), - dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), - {int(dst.getWidth()), int(dst.getHeight())}, GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); -} - -static void FillBuffer(Tensor &dst, const Npp32f value, cudaStream_t stream = 0) -{ - assert(!dst.isCPU()); - const Npp32f padding[3] = {value, value, value}; - NppStatus status = nppiSet_32f_C3R_Ctx(padding, static_cast(dst.getData()), - dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), - {int(dst.getWidth()), int(dst.getHeight())}, GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); -} - -template -void CropAndResizeC1U8Impl(Tensor &dst, const Tensor &src, const BBox &dstROI, - const BBox &srcROI, InterpolationType type, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - AssertValidROI(dstROI, dst.getWidth(), dst.getHeight()); - AssertValidROI(srcROI, src.getWidth(), src.getHeight()); - - NppStatus status = nppiResizeSqrPixel_8u_C1R_Ctx( - static_cast(src.getData() + srcROI.ymin * src.getStride(TensorDimension::HEIGHT) + srcROI.xmin), - {srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), - {0, 0, srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, - static_cast(dst.getData() + dstROI.ymin * dst.getStride(TensorDimension::HEIGHT) + dstROI.xmin), - dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), - {0, 0, dstROI.xmax - dstROI.xmin, dstROI.ymax - dstROI.ymin}, - double(dstROI.xmax - dstROI.xmin) / double(srcROI.xmax - srcROI.xmin), - double(dstROI.ymax - dstROI.ymin) / double(srcROI.ymax - srcROI.ymin), 0.0, 0.0, GetNppiInterpolationMode(type), - GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); -} - -template -void CropAndResizeC1U16Impl(Tensor &dst, const Tensor &src, const BBox &dstROI, - const BBox &srcROI, InterpolationType type, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - AssertValidROI(dstROI, dst.getWidth(), dst.getHeight()); - AssertValidROI(srcROI, src.getWidth(), src.getHeight()); - - NppStatus status = nppiResizeSqrPixel_16u_C1R_Ctx( - static_cast(src.getData() + srcROI.ymin * src.getStride(TensorDimension::HEIGHT) + srcROI.xmin), - {srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), - {0, 0, srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, - static_cast(dst.getData() + dstROI.ymin * dst.getStride(TensorDimension::HEIGHT) + dstROI.xmin), - dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), - {0, 0, dstROI.xmax - dstROI.xmin, dstROI.ymax - dstROI.ymin}, - double(dstROI.xmax - dstROI.xmin) / double(srcROI.xmax - srcROI.xmin), - double(dstROI.ymax - dstROI.ymin) / double(srcROI.ymax - srcROI.ymin), 0.0, 0.0, GetNppiInterpolationMode(type), - GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); -} - -template -void CropAndResizeC1F32Impl(Tensor &dst, const Tensor &src, const BBox &dstROI, - const BBox &srcROI, InterpolationType type, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - AssertValidROI(dstROI, dst.getWidth(), dst.getHeight()); - AssertValidROI(srcROI, src.getWidth(), src.getHeight()); - - NppStatus status = nppiResizeSqrPixel_32f_C1R_Ctx( - static_cast(src.getData() + srcROI.ymin * src.getStride(TensorDimension::HEIGHT) + srcROI.xmin), - {srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), - {0, 0, srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, - static_cast(dst.getData() + dstROI.ymin * dst.getStride(TensorDimension::HEIGHT) + dstROI.xmin), - dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), - {0, 0, dstROI.xmax - dstROI.xmin, dstROI.ymax - dstROI.ymin}, - double(dstROI.xmax - dstROI.xmin) / double(srcROI.xmax - srcROI.xmin), - double(dstROI.ymax - dstROI.ymin) / double(srcROI.ymax - srcROI.ymin), 0.0, 0.0, GetNppiInterpolationMode(type), - GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); -} - -template -void ResizeImpl(Tensor &dst, const Tensor &src, bool keep_aspect_ratio, InterpolationType type, - cudaStream_t stream) -{ - const BBox dstROI = keep_aspect_ratio - ? GetScaledROI(src.getWidth(), src.getHeight(), dst.getWidth(), dst.getHeight()) - : BBox{0, 0, int(dst.getWidth()), int(dst.getHeight())}; - if (keep_aspect_ratio) - { - FillBuffer(dst, 0, stream); - } - CropAndResize(dst, src, dstROI, {0, 0, int(src.getWidth()), int(src.getHeight())}, type, stream); -} - -template -void CropC1U8Impl(Tensor &dst, const Tensor &src, const BBox &srcROI, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - AssertValidROI(srcROI, src.getWidth(), src.getHeight()); - assert(srcROI.xmax - srcROI.xmin == dst.getWidth() && srcROI.ymax - srcROI.ymin == dst.getHeight()); - - NppStatus status = nppiCopy_8u_C1R_Ctx( - static_cast(src.getData() + srcROI.ymin * src.getStride(TensorDimension::HEIGHT) + srcROI.xmin), - src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), static_cast(dst.getData()), - dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), {srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, - GetNppStreamContext(stream)); - - assert(status == NPP_SUCCESS); -} - -template -void CropC1U16Impl(Tensor &dst, const Tensor &src, const BBox &srcROI, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - AssertValidROI(srcROI, src.getWidth(), src.getHeight()); - assert(srcROI.xmax - srcROI.xmin == dst.getWidth() && srcROI.ymax - srcROI.ymin == dst.getHeight()); - - NppStatus status = nppiCopy_16u_C1R_Ctx( - static_cast(src.getData() + srcROI.ymin * src.getStride(TensorDimension::HEIGHT) + srcROI.xmin), - src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), static_cast(dst.getData()), - dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), {srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, - GetNppStreamContext(stream)); - - assert(status == NPP_SUCCESS); -} - -template -void CropC1F32Impl(Tensor &dst, const Tensor &src, const BBox &srcROI, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - AssertValidROI(srcROI, src.getWidth(), src.getHeight()); - assert(srcROI.xmax - srcROI.xmin == dst.getWidth() && srcROI.ymax - srcROI.ymin == dst.getHeight()); - - NppStatus status = nppiCopy_32f_C1R_Ctx( - static_cast(src.getData() + srcROI.ymin * src.getStride(TensorDimension::HEIGHT) + srcROI.xmin), - src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), static_cast(dst.getData()), - dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), {srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, - GetNppStreamContext(stream)); - - assert(status == NPP_SUCCESS); -} - -template -void ResizeBatch(Tensor &dst, Tensor &src, bool keep_aspect_ratio, InterpolationType type, - cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - assert(src.getDepth() == dst.getDepth()); - - for (int i = 0; i < src.getDepth(); i++) - { - size_t offsetSrc = i * src.getStride(TensorDimension::DEPTH); - size_t offsetDst = i * dst.getStride(TensorDimension::DEPTH); - Tensor srcTmp(src.getWidth(), src.getHeight(), - src.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), - src.getData() + offsetSrc, false); - Tensor dstTmp(dst.getWidth(), dst.getHeight(), - dst.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), - dst.getData() + offsetDst, false); - Resize(dstTmp, srcTmp, keep_aspect_ratio, type, stream); - } -} - -} // anonymous namespace - -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio, InterpolationType type, - cudaStream_t stream) -{ - ResizeImpl(dst, src, keep_aspect_ratio, type, stream); -} - -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio, InterpolationType type, - cudaStream_t stream) -{ - ResizeBatch(dst, const_cast &>(src), keep_aspect_ratio, type, stream); -} - -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &dstROI, const BBox &srcROI, - InterpolationType type, cudaStream_t stream) -{ - CropAndResizeC1U8Impl(dst, src, dstROI, srcROI, type, stream); -} - -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &srcROI, InterpolationType type, - cudaStream_t stream) -{ - CropAndResize(dst, src, {0, 0, int(dst.getWidth()), int(dst.getHeight())}, srcROI, type, stream); -} - -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio, InterpolationType type, - cudaStream_t stream) -{ - ResizeImpl(dst, src, keep_aspect_ratio, type, stream); -} - -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio, - InterpolationType type, cudaStream_t stream) -{ - ResizeBatch(dst, const_cast &>(src), keep_aspect_ratio, type, stream); -} - -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &dstROI, const BBox &srcROI, - InterpolationType type, cudaStream_t stream) -{ - CropAndResizeC1U16Impl(dst, src, dstROI, srcROI, type, stream); -} - -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &srcROI, - InterpolationType type, cudaStream_t stream) -{ - CropAndResize(dst, src, {0, 0, int(dst.getWidth()), int(dst.getHeight())}, srcROI, type, stream); -} - -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio, InterpolationType type, - cudaStream_t stream) -{ - ResizeImpl(dst, src, keep_aspect_ratio, type, stream); -} - -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio, - InterpolationType type, cudaStream_t stream) -{ - ResizeBatch(dst, const_cast &>(src), keep_aspect_ratio, type, stream); -} - -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &dstROI, const BBox &srcROI, - InterpolationType type, cudaStream_t stream) -{ - CropAndResizeC1F32Impl(dst, src, dstROI, srcROI, type, stream); -} - -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &srcROI, - InterpolationType type, cudaStream_t stream) -{ - CropAndResize(dst, src, {0, 0, int(dst.getWidth()), int(dst.getHeight())}, srcROI, type, stream); -} - -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio, InterpolationType type, - cudaStream_t stream) -{ - ResizeImpl(dst, src, keep_aspect_ratio, type, stream); -} - -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &dstROI, const BBox &srcROI, - InterpolationType type, cudaStream_t stream) -{ - CropAndResizeC1U8Impl(dst, src, dstROI, srcROI, type, stream); -} - -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &srcROI, InterpolationType type, - cudaStream_t stream) -{ - CropAndResize(dst, src, {0, 0, int(dst.getWidth()), int(dst.getHeight())}, srcROI, type, stream); -} - -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio, InterpolationType type, - cudaStream_t stream) -{ - ResizeImpl(dst, src, keep_aspect_ratio, type, stream); -} - -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &dstROI, const BBox &srcROI, - InterpolationType type, cudaStream_t stream) -{ - CropAndResizeC1U16Impl(dst, src, dstROI, srcROI, type, stream); -} - -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &srcROI, - InterpolationType type, cudaStream_t stream) -{ - CropAndResize(dst, src, {0, 0, int(dst.getWidth()), int(dst.getHeight())}, srcROI, type, stream); -} - -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio, InterpolationType type, - cudaStream_t stream) -{ - ResizeImpl(dst, src, keep_aspect_ratio, type, stream); -} - -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &dstROI, const BBox &srcROI, - InterpolationType type, cudaStream_t stream) -{ - CropAndResizeC1F32Impl(dst, src, dstROI, srcROI, type, stream); -} - -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &srcROI, - InterpolationType type, cudaStream_t stream) -{ - CropAndResize(dst, src, {0, 0, int(dst.getWidth()), int(dst.getHeight())}, srcROI, type, stream); -} - -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio, InterpolationType type, - cudaStream_t stream) -{ - ResizeImpl(dst, src, keep_aspect_ratio, type, stream); -} - -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio, InterpolationType type, - cudaStream_t stream) -{ - ResizeBatch(dst, const_cast &>(src), keep_aspect_ratio, type, stream); -} - -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &dstROI, const BBox &srcROI, - InterpolationType type, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - AssertValidROI(dstROI, dst.getWidth(), dst.getHeight()); - AssertValidROI(srcROI, src.getWidth(), src.getHeight()); - - NppStatus status = nppiResizeSqrPixel_8u_C3R_Ctx( - static_cast(src.getData() + srcROI.ymin * src.getStride(TensorDimension::HEIGHT) + - srcROI.xmin * src.getChannelCount()), - {srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), - {0, 0, srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, - static_cast(dst.getData() + dstROI.ymin * dst.getStride(TensorDimension::HEIGHT) + - dstROI.xmin * dst.getChannelCount()), - dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), - {0, 0, dstROI.xmax - dstROI.xmin, dstROI.ymax - dstROI.ymin}, - double(dstROI.xmax - dstROI.xmin) / double(srcROI.xmax - srcROI.xmin), - double(dstROI.ymax - dstROI.ymin) / double(srcROI.ymax - srcROI.ymin), 0.0, 0.0, GetNppiInterpolationMode(type), - GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); -} - -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &srcROI, InterpolationType type, - cudaStream_t stream) -{ - CropAndResize(dst, src, {0, 0, int(dst.getWidth()), int(dst.getHeight())}, srcROI, type, stream); -} - -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio, InterpolationType type, - cudaStream_t stream) -{ - ResizeImpl(dst, src, keep_aspect_ratio, type, stream); -} - -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio, - InterpolationType type, cudaStream_t stream) -{ - ResizeBatch(dst, const_cast &>(src), keep_aspect_ratio, type, stream); -} - -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &dstROI, const BBox &srcROI, - InterpolationType type, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - AssertValidROI(dstROI, dst.getWidth(), dst.getHeight()); - AssertValidROI(srcROI, src.getWidth(), src.getHeight()); - - NppStatus status = nppiResizeSqrPixel_16u_C3R_Ctx( - static_cast(src.getData() + srcROI.ymin * src.getStride(TensorDimension::HEIGHT) + - srcROI.xmin * src.getChannelCount()), - {srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), - {0, 0, srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, - static_cast(dst.getData() + dstROI.ymin * dst.getStride(TensorDimension::HEIGHT) + - dstROI.xmin * dst.getChannelCount()), - dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), - {0, 0, dstROI.xmax - dstROI.xmin, dstROI.ymax - dstROI.ymin}, - double(dstROI.xmax - dstROI.xmin) / double(srcROI.xmax - srcROI.xmin), - double(dstROI.ymax - dstROI.ymin) / double(srcROI.ymax - srcROI.ymin), 0.0, 0.0, GetNppiInterpolationMode(type), - GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); -} - -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &srcROI, - InterpolationType type, cudaStream_t stream) -{ - CropAndResize(dst, src, {0, 0, int(dst.getWidth()), int(dst.getHeight())}, srcROI, type, stream); -} - -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio, InterpolationType type, - cudaStream_t stream) -{ - ResizeImpl(dst, src, keep_aspect_ratio, type, stream); -} - -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio, - InterpolationType type, cudaStream_t stream) -{ - ResizeBatch(dst, const_cast &>(src), keep_aspect_ratio, type, stream); -} - -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &dstROI, const BBox &srcROI, - InterpolationType type, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - AssertValidROI(dstROI, dst.getWidth(), dst.getHeight()); - AssertValidROI(srcROI, src.getWidth(), src.getHeight()); - - NppStatus status = nppiResizeSqrPixel_32f_C3R_Ctx( - static_cast(src.getData() + srcROI.ymin * src.getStride(TensorDimension::HEIGHT) + - srcROI.xmin * src.getChannelCount()), - {srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), - {0, 0, srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, - static_cast(dst.getData() + dstROI.ymin * dst.getStride(TensorDimension::HEIGHT) + - dstROI.xmin * dst.getChannelCount()), - dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), - {0, 0, dstROI.xmax - dstROI.xmin, dstROI.ymax - dstROI.ymin}, - double(dstROI.xmax - dstROI.xmin) / double(srcROI.xmax - srcROI.xmin), - double(dstROI.ymax - dstROI.ymin) / double(srcROI.ymax - srcROI.ymin), 0.0, 0.0, GetNppiInterpolationMode(type), - GetNppStreamContext(stream)); - assert(status == NPP_SUCCESS); -} - -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &srcROI, - InterpolationType type, cudaStream_t stream) -{ - CropAndResize(dst, src, {0, 0, int(dst.getWidth()), int(dst.getHeight())}, srcROI, type, stream); -} - -void Crop(Tensor &dst, const Tensor &src, const BBox &srcROI, cudaStream_t stream) -{ - CropC1U8Impl(dst, src, srcROI, stream); -} - -void Crop(Tensor &dst, const Tensor &src, const BBox &srcROI, cudaStream_t stream) -{ - CropC1U16Impl(dst, src, srcROI, stream); -} - -void Crop(Tensor &dst, const Tensor &src, const BBox &srcROI, cudaStream_t stream) -{ - CropC1F32Impl(dst, src, srcROI, stream); -} - -void Crop(Tensor &dst, const Tensor &src, const BBox &srcROI, cudaStream_t stream) -{ - CropC1U8Impl(dst, src, srcROI, stream); -} - -void Crop(Tensor &dst, const Tensor &src, const BBox &srcROI, cudaStream_t stream) -{ - CropC1U16Impl(dst, src, srcROI, stream); -} - -void Crop(Tensor &dst, const Tensor &src, const BBox &srcROI, cudaStream_t stream) -{ - CropC1F32Impl(dst, src, srcROI, stream); -} - -void Crop(Tensor &dst, const Tensor &src, const BBox &srcROI, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - AssertValidROI(srcROI, src.getWidth(), src.getHeight()); - assert(srcROI.xmax - srcROI.xmin == dst.getWidth() && srcROI.ymax - srcROI.ymin == dst.getHeight()); - - NppStatus status = nppiCopy_8u_C3R_Ctx( - static_cast(src.getData() + srcROI.ymin * src.getStride(TensorDimension::HEIGHT) + - srcROI.xmin * src.getChannelCount()), - src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), static_cast(dst.getData()), - dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), {srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, - GetNppStreamContext(stream)); - - assert(status == NPP_SUCCESS); -} - -void Crop(Tensor &dst, const Tensor &src, const BBox &srcROI, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - AssertValidROI(srcROI, src.getWidth(), src.getHeight()); - assert(srcROI.xmax - srcROI.xmin == dst.getWidth() && srcROI.ymax - srcROI.ymin == dst.getHeight()); - - NppStatus status = nppiCopy_16u_C3R_Ctx( - static_cast(src.getData() + srcROI.ymin * src.getStride(TensorDimension::HEIGHT) + - srcROI.xmin * src.getChannelCount()), - src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), static_cast(dst.getData()), - dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), {srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, - GetNppStreamContext(stream)); - - assert(status == NPP_SUCCESS); -} - -void Crop(Tensor &dst, const Tensor &src, const BBox &srcROI, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - AssertValidROI(srcROI, src.getWidth(), src.getHeight()); - assert(srcROI.xmax - srcROI.xmin == dst.getWidth() && srcROI.ymax - srcROI.ymin == dst.getHeight()); - - NppStatus status = nppiCopy_32f_C3R_Ctx( - static_cast(src.getData() + srcROI.ymin * src.getStride(TensorDimension::HEIGHT) + - srcROI.xmin * src.getChannelCount()), - src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), static_cast(dst.getData()), - dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), {srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, - GetNppStreamContext(stream)); - - assert(status == NPP_SUCCESS); -} - -void WarpPerspective(Tensor &dst, const Tensor &src, const double coeffs[3][3], - InterpolationType type, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - - NppStatus status = nppiWarpPerspective_8u_C1R_Ctx( - static_cast(src.getData()), {int(src.getWidth()), int(src.getHeight())}, - src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), {0, 0, int(src.getWidth()), int(src.getHeight())}, - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), - {0, 0, int(dst.getWidth()), int(dst.getHeight())}, coeffs, GetNppiInterpolationMode(type), - GetNppStreamContext(stream)); - - assert(status == NPP_SUCCESS); -} - -void WarpPerspective(Tensor &dst, const Tensor &src, const double coeffs[3][3], - InterpolationType type, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - - NppStatus status = nppiWarpPerspective_16u_C1R_Ctx( - static_cast(src.getData()), {int(src.getWidth()), int(src.getHeight())}, - src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), {0, 0, int(src.getWidth()), int(src.getHeight())}, - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), - {0, 0, int(dst.getWidth()), int(dst.getHeight())}, coeffs, GetNppiInterpolationMode(type), - GetNppStreamContext(stream)); - - assert(status == NPP_SUCCESS); -} - -void WarpPerspective(Tensor &dst, const Tensor &src, const double coeffs[3][3], - InterpolationType type, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - - NppStatus status = nppiWarpPerspective_32f_C1R_Ctx( - static_cast(src.getData()), {int(src.getWidth()), int(src.getHeight())}, - src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), {0, 0, int(src.getWidth()), int(src.getHeight())}, - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), - {0, 0, int(dst.getWidth()), int(dst.getHeight())}, coeffs, GetNppiInterpolationMode(type), - GetNppStreamContext(stream)); - - assert(status == NPP_SUCCESS); -} - -void WarpPerspective(Tensor &dst, const Tensor &src, const double coeffs[3][3], - InterpolationType type, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - - NppStatus status = nppiWarpPerspective_8u_C3R_Ctx( - static_cast(src.getData()), {int(src.getWidth()), int(src.getHeight())}, - src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), {0, 0, int(src.getWidth()), int(src.getHeight())}, - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), - {0, 0, int(dst.getWidth()), int(dst.getHeight())}, coeffs, GetNppiInterpolationMode(type), - GetNppStreamContext(stream)); - - assert(status == NPP_SUCCESS); -} - -void WarpPerspective(Tensor &dst, const Tensor &src, const double coeffs[3][3], - InterpolationType type, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - - NppStatus status = nppiWarpPerspective_16u_C3R_Ctx( - static_cast(src.getData()), {int(src.getWidth()), int(src.getHeight())}, - src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), {0, 0, int(src.getWidth()), int(src.getHeight())}, - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), - {0, 0, int(dst.getWidth()), int(dst.getHeight())}, coeffs, GetNppiInterpolationMode(type), - GetNppStreamContext(stream)); - - assert(status == NPP_SUCCESS); -} - -void WarpPerspective(Tensor &dst, const Tensor &src, const double coeffs[3][3], - InterpolationType type, cudaStream_t stream) -{ - // src and dst must be GPU tensors - assert(!src.isCPU() && !dst.isCPU()); - - NppStatus status = nppiWarpPerspective_32f_C3R_Ctx( - static_cast(src.getData()), {int(src.getWidth()), int(src.getHeight())}, - src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), {0, 0, int(src.getWidth()), int(src.getHeight())}, - static_cast(dst.getData()), dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), - {0, 0, int(dst.getWidth()), int(dst.getHeight())}, coeffs, GetNppiInterpolationMode(type), - GetNppStreamContext(stream)); - - assert(status == NPP_SUCCESS); -} - -}} // namespace cvcore::tensor_ops \ No newline at end of file diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIRemapImpl.h b/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIRemapImpl.h deleted file mode 100644 index e129f04..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIRemapImpl.h +++ /dev/null @@ -1,82 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 - -#ifndef CVCORE_VPIREMAPIMPL_H -#define CVCORE_VPIREMAPIMPL_H - -#include -#include - -#include "VPITensorOperators.h" -#include "VPIImageWarp.h" - -namespace cvcore { namespace tensor_ops { -/** - * Remap implementation used for Lens Distortion. - */ -class VPITensorStream::VPIRemapImpl -{ - public: - /* VPIRemapImpl constructor */ - VPIRemapImpl(); - - /** - * Remap Intialization. - * @param outputImage Remap output image of Type - * @param inputImage Remap input image of Type - * @param backend Compute backend - * @return Success if intialization is done successfully, otherwise error is returned - */ - template - std::error_code initialize(Image & outImage, - const Image & inImage, - VPIBackend backend); - - /** - * Remap execution function(non-blocking) - * Application is reqiured to call Sync() before accessing the generated Image. - * @param outImage Remap output image of type NV12 - * @param inImage Remap input image of type NV12 - * @param warp Remap warp pointer - * @param interpolation Interpolation type used for remap - * @param border Border type used for remap - * @param stream VPI stream used for remap - * @param backend VPI backend used for remap - * @return Success if remap is submitted successfully, otherwise error is returned - */ - template - std::error_code execute(Image & outImage, - const Image & inImage, - const VPIImageWarp * warp, - InterpolationType interpolation, - BorderType border, - VPIStream & stream, - VPIBackend backend); - - /* VPIRemapImpl destructor to release resources */ - ~VPIRemapImpl(); - - private: - VPIImage m_inputImage; - VPIImage m_outputImage; - VPIImageData m_inputImageData; - VPIImageData m_outputImageData; -}; - -}} // namespace cvcore::tensor_ops - -#endif //CVCORE_VPIREMAPIMPL_H diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIResizeImpl.cpp b/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIResizeImpl.cpp deleted file mode 100644 index 1adffa3..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIResizeImpl.cpp +++ /dev/null @@ -1,139 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 -#include "VPIResizeImpl.h" - -#include -#include -#include -#include - -// VPI includes -#include -#include -#include -#include -#include - -#include "VPIEnumMapping.h" -#include "VPIStatusMapping.h" -#include "cv/core/CameraModel.h" -#include "cv/core/Image.h" - -#ifdef NVBENCH_ENABLE -#include -#endif - -namespace cvcore { namespace tensor_ops { - -template -std::error_code VPITensorStream::VPIResizeImpl::execute(Image &outputImage, const Image &inputImage, - InterpolationType interpolation, BorderType border, - VPIStream &stream, VPIBackend backend) -{ - std::error_code errCode = ErrorCode::SUCCESS; - VPIStatus status = VPIStatus::VPI_SUCCESS; - VPIInterpolationType interpType; - VPIBorderExtension borderExt; - interpType = ToVpiInterpolationType(interpolation); - borderExt = ToVpiBorderType(border); - - bool paramsChanged = m_inputImage == nullptr || m_outputImage == nullptr || - CheckParamsChanged(m_inputImageData, inputImage) || - CheckParamsChanged(m_outputImageData, outputImage); - if (paramsChanged) - { - DestroyVPIImageWrapper(m_inputImage, m_inputImageData); - DestroyVPIImageWrapper(m_outputImage, m_outputImageData); - errCode = CreateVPIImageWrapper(m_inputImage, m_inputImageData, inputImage, backend); - if (errCode == make_error_code(VPI_SUCCESS)) - { - errCode = CreateVPIImageWrapper(m_outputImage, m_outputImageData, outputImage, backend); - } - } - else - { - - errCode = UpdateImage(m_inputImage, m_inputImageData, inputImage); - if (errCode == make_error_code(VPIStatus::VPI_SUCCESS)) - { - errCode = UpdateImage(m_outputImage, m_outputImageData, outputImage); - } - } - - if (status == VPIStatus::VPI_SUCCESS) - { -#ifdef NVBENCH_ENABLE - std::string tag = "VPISubmitRescale_" + GetMemoryTypeAsString(inputImage.isCPU()) +"Input_" + GetMemoryTypeAsString(outputImage.isCPU()) +"Output_" + getVPIBackendString(backend) + "Backend"; - nv::bench::Timer timerFunc = - nv::bench::VPI(tag.c_str(), nv::bench::Flag::DEFAULT, stream); -#endif - // Resize - status = vpiSubmitRescale(stream, backend, m_inputImage, m_outputImage, interpType, borderExt, 0); - } - - if (status == VPIStatus::VPI_SUCCESS) - { - status = vpiStreamSync(stream); - } - - if (status != VPIStatus::VPI_SUCCESS) - { - return make_error_code(status); - } - return make_error_code(ErrorCode::SUCCESS); -} - -VPITensorStream::VPIResizeImpl::VPIResizeImpl() - : m_inputImage(nullptr) - , m_outputImage(nullptr) -{ - std::memset(reinterpret_cast(&m_inputImageData), 0, sizeof(VPIImageData)); - std::memset(reinterpret_cast(&m_outputImageData), 0, sizeof(VPIImageData)); -} - -/** -* Image resizing destroy function to deallocate resources. -*/ -VPITensorStream::VPIResizeImpl::~VPIResizeImpl() -{ - // Destroy Input VPIImage - DestroyVPIImageWrapper(m_inputImage, m_inputImageData); - // Destroy Output VPIImage - DestroyVPIImageWrapper(m_outputImage, m_outputImageData); -} - -template std::error_code VPITensorStream::VPIResizeImpl::execute(Image &outputImage, - const Image &inputImage, - InterpolationType interpolation, BorderType border, - VPIStream &stream, VPIBackend backend); -template std::error_code VPITensorStream::VPIResizeImpl::execute(Image &outputImage, - const Image &inputImage, - InterpolationType interpolation, BorderType border, - VPIStream &stream, VPIBackend backend); -template std::error_code VPITensorStream::VPIResizeImpl::execute(Image &outputImage, - const Image &inputImage, - InterpolationType interpolation, BorderType border, - VPIStream &stream, VPIBackend backend); -template std::error_code VPITensorStream::VPIResizeImpl::execute(Image &outputImage, - const Image &inputImage, - InterpolationType interpolation, BorderType border, - VPIStream &stream, VPIBackend backend); -template std::error_code VPITensorStream::VPIResizeImpl::execute(Image &outputImage, - const Image &inputImage, - InterpolationType interpolation, BorderType border, - VPIStream &stream, VPIBackend backend); -}} // namespace cvcore::tensor_ops diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIStatusMapping.cpp b/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIStatusMapping.cpp deleted file mode 100644 index a43431d..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIStatusMapping.cpp +++ /dev/null @@ -1,122 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 - -#include "cv/tensor_ops/Errors.h" -#include "vpi/Status.h" - -#ifndef __cpp_lib_to_underlying -// Using a C++23 feature by hacking std -namespace std -{ - template - constexpr underlying_type_t to_underlying(Enum e) noexcept - { - return static_cast>(e); - } -}; -#endif // __cpp_lib_to_underlying - -namespace cvcore { namespace tensor_ops { - -namespace detail -{ - struct VPIStatusCategory : std::error_category - { - virtual const char * name() const noexcept override final - { - return "vpi-status"; - } - - virtual std::string message(int value) const override final - { - std::string result = "VPI Status"; - - return result; - } - - virtual std::error_condition default_error_condition(int code) const noexcept override final - { - std::error_condition result; - - switch(code) - { - case VPI_SUCCESS: - result = ErrorCode::SUCCESS; - break; - - case VPI_ERROR_INVALID_ARGUMENT: - result = ErrorCode::INVALID_ARGUMENT; - break; - - case VPI_ERROR_INVALID_IMAGE_FORMAT: - result = ErrorCode::INVALID_IMAGE_FORMAT; - break; - - case VPI_ERROR_INVALID_ARRAY_TYPE: - result = ErrorCode::INVALID_STORAGE_TYPE; - break; - - case VPI_ERROR_INVALID_PAYLOAD_TYPE: - result = ErrorCode::INVALID_STORAGE_TYPE; - break; - - case VPI_ERROR_INVALID_OPERATION: - result = ErrorCode::INVALID_OPERATION; - break; - - case VPI_ERROR_INVALID_CONTEXT: - result = ErrorCode::INVALID_ENGINE_TYPE; - break; - - case VPI_ERROR_DEVICE: - result = ErrorCode::DEVICE_ERROR; - break; - - case VPI_ERROR_NOT_READY: - result = ErrorCode::NOT_READY; - break; - - case VPI_ERROR_BUFFER_LOCKED: - result = ErrorCode::SYSTEM_ERROR; - break; - - case VPI_ERROR_OUT_OF_MEMORY: - result = ErrorCode::OUT_OF_MEMORY; - break; - - case VPI_ERROR_INTERNAL: - result = ErrorCode::SYSTEM_ERROR; - break; - - default: - result = ErrorCode::NOT_IMPLEMENTED; - break; - } - - return result; - } - }; -} // namespace detail - -const detail::VPIStatusCategory errorCategory{}; - -std::error_code make_error_code(VPIStatus ec) noexcept -{ - return {std::to_underlying(ec), errorCategory}; -} - -}} // namespace cvcore::tensor_ops diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIStereoDisparityEstimatorImpl.cpp b/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIStereoDisparityEstimatorImpl.cpp deleted file mode 100644 index c4e96e4..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIStereoDisparityEstimatorImpl.cpp +++ /dev/null @@ -1,211 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 - -#include -#include - -#include "VPIStereoDisparityEstimatorImpl.h" -#include "VPIEnumMapping.h" -#include "VPIStatusMapping.h" - -#include "cv/core/CameraModel.h" -#include "cv/core/Image.h" - -#include - -#ifdef NVBENCH_ENABLE -#include -#endif - -namespace cvcore { namespace tensor_ops { - -VPITensorStream::VPIStereoDisparityEstimatorImpl::VPIStereoDisparityEstimatorImpl() - : m_inputLeftImage(nullptr) - , m_inputRightImage(nullptr) - , m_outputImage(nullptr) - , m_tempImage(nullptr) - , m_payload(nullptr) - , m_stereoParams() -{ - std::memset(reinterpret_cast(&m_inputLeftImageData), 0, sizeof(VPIImageData)); - std::memset(reinterpret_cast(&m_inputRightImageData), 0, sizeof(VPIImageData)); - std::memset(reinterpret_cast(&m_outputImageData), 0, sizeof(VPIImageData)); - // Disparity values returned from VPI are in Q10.5 format, i.e., signed fixed point with 5 fractional bits. Divide it by 32.0f to convert it to floating point. - vpiInitConvertImageFormatParams(&m_cvtParams); - m_cvtParams.scale = 1.0f / 32; -} - -template -std::error_code VPITensorStream::VPIStereoDisparityEstimatorImpl::initialize(Image &outImage, - const Image &leftImage, - const Image &rightImage, - VPIBackend backend) -{ - std::error_code status; - const std::error_code success = make_error_code(VPI_SUCCESS); - status = CreateVPIImageWrapper(m_inputLeftImage, m_inputLeftImageData, leftImage, backend); - if (status == success) - { - status = CreateVPIImageWrapper(m_inputRightImage, m_inputRightImageData, rightImage, backend); - } - if (status == success) - { - status = CreateVPIImageWrapper(m_outputImage, m_outputImageData, outImage, backend); - } - if (status == success) - { - status = make_error_code( - vpiImageCreate(outImage.getWidth(), outImage.getHeight(), VPI_IMAGE_FORMAT_S16, 0, &m_tempImage)); - } - if (status == success) - { - status = make_error_code(vpiCreateStereoDisparityEstimator(backend, outImage.getWidth(), outImage.getHeight(), - ToVpiImageFormat(T_IN), NULL, &m_payload)); - } - return status; -} - -template std::error_code VPITensorStream::VPIStereoDisparityEstimatorImpl::initialize(Image &outImage, - const Image &leftImage, - const Image &rightImage, - VPIBackend backend); -template std::error_code VPITensorStream::VPIStereoDisparityEstimatorImpl::initialize(Image &outImage, - const Image &leftImage, - const Image &rightImage, - VPIBackend backend); -template std::error_code VPITensorStream::VPIStereoDisparityEstimatorImpl::initialize(Image &outImage, - const Image &leftImage, - const Image &rightImage, - VPIBackend backend); - -// ----------------------------------------------------------------------------- - -template -std::error_code VPITensorStream::VPIStereoDisparityEstimatorImpl::execute(Image &outImage, - const Image &leftImage, - const Image &rightImage, - size_t windowSize, size_t maxDisparity, - VPIStream &stream, VPIBackend backend) -{ - std::error_code status = make_error_code(VPI_SUCCESS); - m_stereoParams.windowSize = static_cast(windowSize); - m_stereoParams.maxDisparity = static_cast(maxDisparity); - - bool paramsChanged = m_inputLeftImage == nullptr || m_inputRightImage == nullptr || m_outputImage == nullptr || - CheckParamsChanged(m_inputLeftImageData, leftImage) || - CheckParamsChanged(m_inputRightImageData, rightImage) || - CheckParamsChanged(m_outputImageData, outImage); - - if (paramsChanged) - { - if (m_payload != nullptr) - { - vpiPayloadDestroy(m_payload); - } - if (m_tempImage != nullptr) - { - vpiImageDestroy(m_tempImage); - } - DestroyVPIImageWrapper(m_inputLeftImage, m_inputLeftImageData); - DestroyVPIImageWrapper(m_inputRightImage, m_inputRightImageData); - DestroyVPIImageWrapper(m_outputImage, m_outputImageData); - - status = initialize(outImage, leftImage, rightImage, backend); - } - - if (status == make_error_code(VPI_SUCCESS)) - { - status = UpdateImage(m_inputLeftImage, m_inputLeftImageData, leftImage); - } - - if (status == make_error_code(VPI_SUCCESS)) - { - status = UpdateImage(m_inputRightImage, m_inputRightImageData, rightImage); - } - - if (status == make_error_code(VPI_SUCCESS)) - { - status = UpdateImage(m_outputImage, m_outputImageData, outImage); - } - - if (status == make_error_code(VPI_SUCCESS)) - { -#ifdef NVBENCH_ENABLE - std::string tag = "VPISubmitStereoDisparityEstimator_" + GetMemoryTypeAsString(leftImage.isCPU()) + "Input_" + - GetMemoryTypeAsString(outImage.isCPU()) + "Output_" + getVPIBackendString(backend) + - "Backend"; - nv::bench::Timer timerFunc = nv::bench::VPI(tag.c_str(), nv::bench::Flag::DEFAULT, stream); -#endif - // Submit SGM task for Stereo Disparity Estimator - status = make_error_code(vpiSubmitStereoDisparityEstimator( - stream, backend, m_payload, m_inputLeftImage, m_inputRightImage, m_tempImage, NULL, &m_stereoParams)); - } - - if (status == make_error_code(VPI_SUCCESS)) - { -#ifdef NVBENCH_ENABLE - std::string tag = "VPISubmitConvertImageFormat_" + GetMemoryTypeAsString(leftImage.isCPU()) + "Input_" + - GetMemoryTypeAsString(outImage.isCPU()) + "Output_" + getVPIBackendString(backend) + - "Backend"; - nv::bench::Timer timerFunc = nv::bench::VPI(tag.c_str(), nv::bench::Flag::DEFAULT, stream); -#endif - // Submit SGM task for Stereo Disparity Estimator - status = - make_error_code(vpiSubmitConvertImageFormat(stream, backend, m_tempImage, m_outputImage, &m_cvtParams)); - } - - if (status == make_error_code(VPI_SUCCESS)) - { - // Wait for stereo disparity estimator to complete - status = make_error_code(vpiStreamSync(stream)); - } - - if (status != make_error_code(VPI_SUCCESS)) - { - return status; - } - return make_error_code(ErrorCode::SUCCESS); -} - -template std::error_code VPITensorStream::VPIStereoDisparityEstimatorImpl::execute( - Image &outImage, const Image &leftImage, const Image &rightImage, size_t windowSize, - size_t maxDisparity, VPIStream &stream, VPIBackend backend); -template std::error_code VPITensorStream::VPIStereoDisparityEstimatorImpl::execute( - Image &outImage, const Image &leftImage, const Image &rightImage, size_t windowSize, - size_t maxDisparity, VPIStream &stream, VPIBackend backend); -template std::error_code VPITensorStream::VPIStereoDisparityEstimatorImpl::execute( - Image &outImage, const Image &leftImage, const Image &rightImage, size_t windowSize, - size_t maxDisparity, VPIStream &stream, VPIBackend backend); -// ----------------------------------------------------------------------------- - -VPITensorStream::VPIStereoDisparityEstimatorImpl::~VPIStereoDisparityEstimatorImpl() -{ - if (m_payload != nullptr) - { - vpiPayloadDestroy(m_payload); - } - if (m_tempImage != nullptr) - { - vpiImageDestroy(m_tempImage); - } - DestroyVPIImageWrapper(m_inputLeftImage, m_inputLeftImageData); - DestroyVPIImageWrapper(m_inputRightImage, m_inputRightImageData); - DestroyVPIImageWrapper(m_outputImage, m_outputImageData); -} -// ----------------------------------------------------------------------------- - -}} // namespace cvcore::tensor_ops diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPITensorOperators.cpp b/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPITensorOperators.cpp deleted file mode 100644 index 83c5dff..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPITensorOperators.cpp +++ /dev/null @@ -1,709 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "cv/core/CameraModel.h" -#include "cv/core/Image.h" - -#include "VPIColorConvertImpl.h" -#include "VPIEnumMapping.h" -#include "VPIImageWarp.h" -#include "VPIRemapImpl.h" -#include "VPIResizeImpl.h" -#include "VPIStatusMapping.h" -#include "VPIStereoDisparityEstimatorImpl.h" -#include "VPITensorOperators.h" - -#ifdef NVBENCH_ENABLE -#include -#endif - -namespace cvcore { namespace tensor_ops { - -namespace detail { - -// helper function to wrap VPI image for NV12 / NV24 image types -template::value>::type * = nullptr> -std::error_code CreateVPIImageWrapperImpl(VPIImage &vpiImg, VPIImageData &imgdata, const Image &cvcoreImage, VPIBackend backend) -{ -#ifdef NVBENCH_ENABLE - std::string tag = "CreateVPIImageWrapper_NV12/NV24"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - - std::memset(reinterpret_cast(&imgdata), 0, sizeof(VPIImageData)); - - imgdata.bufferType = cvcoreImage.isCPU() ? VPI_IMAGE_BUFFER_HOST_PITCH_LINEAR : VPI_IMAGE_BUFFER_CUDA_PITCH_LINEAR; - imgdata.buffer.pitch.format = ToVpiImageFormat(T); - imgdata.buffer.pitch.numPlanes = 2; - imgdata.buffer.pitch.planes[0].data = const_cast(cvcoreImage.getLumaData()); - imgdata.buffer.pitch.planes[0].height = cvcoreImage.getLumaHeight(); - imgdata.buffer.pitch.planes[0].width = cvcoreImage.getLumaWidth(); - imgdata.buffer.pitch.planes[0].pixelType = VPI_PIXEL_TYPE_U8; - imgdata.buffer.pitch.planes[0].pitchBytes = cvcoreImage.getLumaStride(TensorDimension::HEIGHT) * sizeof(uint8_t); - imgdata.buffer.pitch.planes[1].data = const_cast(cvcoreImage.getChromaData()); - imgdata.buffer.pitch.planes[1].height = cvcoreImage.getChromaHeight(); - imgdata.buffer.pitch.planes[1].width = cvcoreImage.getChromaWidth(); - imgdata.buffer.pitch.planes[1].pixelType = VPI_PIXEL_TYPE_2U8; - imgdata.buffer.pitch.planes[1].pitchBytes = cvcoreImage.getChromaStride(TensorDimension::HEIGHT) * sizeof(uint8_t); - VPIStatus vpiStatus; - vpiStatus = vpiImageCreateWrapper(&imgdata, nullptr, backend, &vpiImg); - - return make_error_code(vpiStatus); -} - -// helper function to wrap VPI image for interleaved image types -template::value>::type * = nullptr> -std::error_code CreateVPIImageWrapperImpl(VPIImage &vpiImg, VPIImageData &imgdata, const Image &cvcoreImage, VPIBackend backend) -{ -#ifdef NVBENCH_ENABLE - std::string tag = "CreateVPIImageWrapper_Interleaved"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - - std::memset(reinterpret_cast(&imgdata), 0, sizeof(VPIImageData)); - - using D = typename Image::DataType; - imgdata.bufferType = cvcoreImage.isCPU() ? VPI_IMAGE_BUFFER_HOST_PITCH_LINEAR : VPI_IMAGE_BUFFER_CUDA_PITCH_LINEAR; - imgdata.buffer.pitch.format = ToVpiImageFormat(T); - imgdata.buffer.pitch.numPlanes = 1; - imgdata.buffer.pitch.planes[0].data = const_cast(cvcoreImage.getData()); - imgdata.buffer.pitch.planes[0].height = cvcoreImage.getHeight(); - imgdata.buffer.pitch.planes[0].width = cvcoreImage.getWidth(); - imgdata.buffer.pitch.planes[0].pixelType = ToVpiPixelType(T); - imgdata.buffer.pitch.planes[0].pitchBytes = cvcoreImage.getStride(TensorDimension::HEIGHT) * GetImageElementSize(T); - VPIStatus vpiStatus; - vpiStatus = vpiImageCreateWrapper(&imgdata, nullptr, backend, &vpiImg); - - return make_error_code(vpiStatus); -} - -// helper function to wrap VPI image for planar image types -template::value>::type * = nullptr> -std::error_code CreateVPIImageWrapperImpl(VPIImage &vpiImg, VPIImageData &imgdata, const Image &cvcoreImage, VPIBackend backend) -{ - return make_error_code(VPI_ERROR_INVALID_IMAGE_FORMAT); -} - -} // namespace detail - -std::error_code VPITensorContext::CreateStream(TensorOperatorStream &tensorStream, const ComputeEngine &computeEngine) -{ - tensorStream = nullptr; - - if (!IsComputeEngineCompatible(computeEngine)) - { - return ErrorCode::INVALID_ENGINE_TYPE; - } - - try - { - tensorStream = new VPITensorStream(computeEngine); - } - catch (std::error_code &e) - { - return e; - } - catch (...) - { - return ErrorCode::INVALID_OPERATION; - } - - return ErrorCode::SUCCESS; -} - -VPITensorStream::VPITensorStream(const ComputeEngine &computeEngine) - : m_resizer(new VPIResizeImpl()) - , m_remapper(new VPIRemapImpl()) - , m_colorConverter(new VPIColorConvertImpl()) - , m_stereoDisparityEstimator(new VPIStereoDisparityEstimatorImpl()) -{ - VPIBackend backend = ToVpiBackendType(computeEngine); - VPIStatus status = vpiStreamCreate(backend, &m_stream); - if (status != VPI_SUCCESS) - { - throw make_error_code(status); - } - m_backend = backend; -} - -VPITensorStream::~VPITensorStream() -{ - vpiStreamDestroy(m_stream); -} - -std::error_code VPITensorContext::DestroyStream(TensorOperatorStream &inputStream) -{ - if (inputStream != nullptr) - { - delete inputStream; - inputStream = nullptr; - } - return ErrorCode::SUCCESS; -} - -bool VPITensorContext::IsComputeEngineCompatible(const ComputeEngine &computeEngine) const noexcept -{ - VPIBackend vpibackend = ToVpiBackendType(computeEngine); - if (vpibackend == VPIBackend::VPI_BACKEND_INVALID) - { - return false; - } - return true; -} - -template -std::error_code CreateVPIImageWrapper(VPIImage &vpiImg, VPIImageData &imgdata, const Image &cvcoreImage, VPIBackend backend) -{ - return detail::CreateVPIImageWrapperImpl(vpiImg, imgdata, cvcoreImage, backend); -} - -template std::error_code CreateVPIImageWrapper(VPIImage &, VPIImageData &, const Image &, VPIBackend); -template std::error_code CreateVPIImageWrapper(VPIImage &, VPIImageData &, const Image &, VPIBackend); -template std::error_code CreateVPIImageWrapper(VPIImage &, VPIImageData &, const Image &, VPIBackend); -template std::error_code CreateVPIImageWrapper(VPIImage &, VPIImageData &, const Image &, VPIBackend); -template std::error_code CreateVPIImageWrapper(VPIImage &, VPIImageData &, const Image &, VPIBackend); -template std::error_code CreateVPIImageWrapper(VPIImage &, VPIImageData &, const Image &, VPIBackend); -template std::error_code CreateVPIImageWrapper(VPIImage &, VPIImageData &, const Image &, VPIBackend); -template std::error_code CreateVPIImageWrapper(VPIImage &, VPIImageData &, const Image &, VPIBackend); -template std::error_code CreateVPIImageWrapper(VPIImage &, VPIImageData &, const Image &, VPIBackend); -template std::error_code CreateVPIImageWrapper(VPIImage &, VPIImageData &, const Image &, VPIBackend); - -std::error_code UpdateVPIImageWrapper(VPIImage &image, VPIImageData &imageWrap, bool isCPU) -{ -#ifdef NVBENCH_ENABLE - std::string tag = "UpdateVPIImageWrapper"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - - VPIStatus status = VPI_SUCCESS; - status = vpiImageSetWrapper(image, &imageWrap); - - return make_error_code(status); -} - -std::error_code DestroyVPIImageWrapper(VPIImage &image, VPIImageData &imageWrap) -{ -#ifdef NVBENCH_ENABLE - std::string tag = "DestroyVPIImageWrapper"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - - std::memset(reinterpret_cast(&imageWrap), 0, sizeof(VPIImageData)); - if (image != nullptr) - { - vpiImageDestroy(image); - } - - image = nullptr; - - return ErrorCode::SUCCESS; -} -std::error_code VPITensorStream::Status() noexcept -{ - return ErrorCode::SUCCESS; -} - -std::error_code VPITensorStream::Resize(Image &outputImage, const Image &inputImage, - InterpolationType interpolation, BorderType border) -{ - std::unique_lock scopedLock{m_fence}; -#ifdef NVBENCH_ENABLE - std::string tag = "VPIResize_RGB_U8_" + std::to_string(inputImage.getWidth()) + "X" + - std::to_string(inputImage.getHeight()) + "_" + std::to_string(outputImage.getWidth()) + "X" + - std::to_string(outputImage.getHeight()) + "_" + getVPIBackendString(m_backend) + "Backend_" + - GetMemoryTypeAsString(inputImage.isCPU()) + "Input"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - - std::error_code err_code; - err_code = m_resizer->execute(outputImage, inputImage, interpolation, border, m_stream, m_backend); - return err_code; -} - -std::error_code VPITensorStream::Resize(Image &outputImage, const Image &inputImage, - InterpolationType interpolation, BorderType border) -{ - std::unique_lock scopedLock{m_fence}; -#ifdef NVBENCH_ENABLE - std::string tag = "VPIResize_NV12_" + std::to_string(inputImage.getLumaWidth()) + "X" + - std::to_string(inputImage.getLumaHeight()) + "_" + std::to_string(outputImage.getLumaWidth()) + - "X" + std::to_string(outputImage.getLumaHeight()) + "_" + getVPIBackendString(m_backend) + - "Backend_" + GetMemoryTypeAsString(inputImage.isCPU()) + "Input"; - ; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - - std::error_code err_code; - err_code = m_resizer->execute(outputImage, inputImage, interpolation, border, m_stream, m_backend); - return err_code; -} - -std::error_code VPITensorStream::Resize(Image &outputImage, const Image &inputImage, - InterpolationType interpolation, BorderType border) -{ - std::unique_lock scopedLock{m_fence}; -#ifdef NVBENCH_ENABLE - std::string tag = "VPIResize_RGBA_U8_" + std::to_string(inputImage.getWidth()) + "X" + - std::to_string(inputImage.getHeight()) + "_" + std::to_string(outputImage.getWidth()) + "X" + - std::to_string(outputImage.getHeight()) + "_" + getVPIBackendString(m_backend) + "Backend_" + - GetMemoryTypeAsString(inputImage.isCPU()) + "Input"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - - std::error_code err_code; - err_code = m_resizer->execute(outputImage, inputImage, interpolation, border, m_stream, m_backend); - return err_code; -} - -std::error_code VPITensorStream::Resize(Image &outputImage, const Image &inputImage, - InterpolationType interpolation, BorderType border) -{ - std::unique_lock scopedLock{m_fence}; -#ifdef NVBENCH_ENABLE - std::string tag = "VPIResize_BGR_U8_" + std::to_string(inputImage.getWidth()) + "X" + - std::to_string(inputImage.getHeight()) + "_" + std::to_string(outputImage.getWidth()) + "X" + - std::to_string(outputImage.getHeight()) + "_" + getVPIBackendString(m_backend) + "Backend_" + - GetMemoryTypeAsString(inputImage.isCPU()) + "Input"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - - std::error_code err_code; - err_code = m_resizer->execute(outputImage, inputImage, interpolation, border, m_stream, m_backend); - return err_code; -} - -std::error_code VPITensorStream::Resize(Image &outputImage, const Image &inputImage, - InterpolationType interpolation, BorderType border) -{ - std::unique_lock scopedLock{m_fence}; -#ifdef NVBENCH_ENABLE - std::string tag = "VPIResize_NV24_" + std::to_string(inputImage.getLumaWidth()) + "X" + - std::to_string(inputImage.getLumaHeight()) + "_" + std::to_string(outputImage.getLumaWidth()) + - "X" + std::to_string(outputImage.getLumaHeight()) + "_" + getVPIBackendString(m_backend) + - "Backend_" + GetMemoryTypeAsString(inputImage.isCPU()) + "Input"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - - std::error_code err_code; - err_code = m_resizer->execute(outputImage, inputImage, interpolation, border, m_stream, m_backend); - return err_code; -} - -std::error_code VPITensorStream::Remap(Image &outputImage, const Image &inputImage, - const ImageWarp warp, InterpolationType interpolation, BorderType border) -{ - std::unique_lock scopedLock{m_fence}; -#ifdef NVBENCH_ENABLE - std::string tag = "VPIRemap_RGB_U8_" + std::to_string(inputImage.getWidth()) + "X" + - std::to_string(inputImage.getHeight()) + "_" + std::to_string(outputImage.getWidth()) + "X" + - std::to_string(outputImage.getHeight()) + "_" + getVPIBackendString(m_backend) + "Backend_" + - GetMemoryTypeAsString(inputImage.isCPU()) + "Input"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - - return m_remapper->execute(outputImage, inputImage, reinterpret_cast(warp), interpolation, border, - m_stream, m_backend); -} - -std::error_code VPITensorStream::Remap(Image &outputImage, const Image &inputImage, - const ImageWarp warp, InterpolationType interpolation, BorderType border) -{ - std::unique_lock scopedLock{m_fence}; -#ifdef NVBENCH_ENABLE - std::string tag = "VPIRemap_BGR_U8_" + std::to_string(inputImage.getWidth()) + "X" + - std::to_string(inputImage.getHeight()) + "_" + std::to_string(outputImage.getWidth()) + "X" + - std::to_string(outputImage.getHeight()) + "_" + getVPIBackendString(m_backend) + "Backend_" + - GetMemoryTypeAsString(inputImage.isCPU()) + "Input"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - - return m_remapper->execute(outputImage, inputImage, reinterpret_cast(warp), interpolation, border, - m_stream, m_backend); -} - -std::error_code VPITensorStream::Remap(Image &outputImage, const Image &inputImage, const ImageWarp warp, - InterpolationType interpolation, BorderType border) -{ - std::unique_lock scopedLock{m_fence}; -#ifdef NVBENCH_ENABLE - std::string tag = "VPIRemap_NV12_" + std::to_string(inputImage.getLumaWidth()) + "X" + - std::to_string(inputImage.getLumaHeight()) + "_" + std::to_string(outputImage.getLumaWidth()) + - "X" + std::to_string(outputImage.getLumaHeight()) + "_" + getVPIBackendString(m_backend) + - "Backend_" + GetMemoryTypeAsString(inputImage.isCPU()) + "Input"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - - return m_remapper->execute(outputImage, inputImage, reinterpret_cast(warp), interpolation, border, - m_stream, m_backend); -} - -std::error_code VPITensorStream::Remap(Image &outputImage, const Image &inputImage, const ImageWarp warp, - InterpolationType interpolation, BorderType border) -{ - std::unique_lock scopedLock{m_fence}; -#ifdef NVBENCH_ENABLE - std::string tag = "VPIRemap_NV24_" + std::to_string(inputImage.getLumaWidth()) + "X" + - std::to_string(inputImage.getLumaHeight()) + "_" + std::to_string(outputImage.getLumaWidth()) + - "X" + std::to_string(outputImage.getLumaHeight()) + "_" + getVPIBackendString(m_backend) + - "Backend_" + GetMemoryTypeAsString(inputImage.isCPU()) + "Input"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - - return m_remapper->execute(outputImage, inputImage, reinterpret_cast(warp), interpolation, border, - m_stream, m_backend); -} - -std::error_code VPITensorStream::ColorConvert(Image &outputImage, const Image &inputImage) -{ - std::unique_lock scopedLock{m_fence}; -#ifdef NVBENCH_ENABLE - std::string tag = "VPIColorConvert_BGR_RGB_" + std::to_string(inputImage.getWidth()) + "X" + - std::to_string(inputImage.getHeight()) + "_" + std::to_string(outputImage.getWidth()) + "X" + - std::to_string(outputImage.getHeight()) + "_" + getVPIBackendString(m_backend) + "Backend_" + - GetMemoryTypeAsString(inputImage.isCPU()) + "Input"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - - return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); -} - -std::error_code VPITensorStream::ColorConvert(Image &outputImage, const Image &inputImage) -{ - std::unique_lock scopedLock{m_fence}; -#ifdef NVBENCH_ENABLE - std::string tag = "VPIColorConvert_RGB_BGR_" + std::to_string(inputImage.getWidth()) + "X" + - std::to_string(inputImage.getHeight()) + "_" + std::to_string(outputImage.getWidth()) + "X" + - std::to_string(outputImage.getHeight()) + "_" + getVPIBackendString(m_backend) + "Backend_" + - GetMemoryTypeAsString(inputImage.isCPU()) + "Input"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - - return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); -} - -std::error_code VPITensorStream::ColorConvert(Image &outputImage, const Image &inputImage) -{ - std::unique_lock scopedLock{m_fence}; -#ifdef NVBENCH_ENABLE - std::string tag = "VPIColorConvert_NV12_BGR_" + std::to_string(inputImage.getWidth()) + "X" + - std::to_string(inputImage.getHeight()) + "_" + std::to_string(outputImage.getLumaWidth()) + "X" + - std::to_string(outputImage.getLumaHeight()) + "_" + getVPIBackendString(m_backend) + "Backend_" + - GetMemoryTypeAsString(inputImage.isCPU()) + "Input"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - - return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); -} - -std::error_code VPITensorStream::ColorConvert(Image &outputImage, const Image &inputImage) -{ - std::unique_lock scopedLock{m_fence}; -#ifdef NVBENCH_ENABLE - std::string tag = "VPIColorConvert_NV24_BGR_" + std::to_string(inputImage.getWidth()) + "X" + - std::to_string(inputImage.getHeight()) + "_" + std::to_string(outputImage.getLumaWidth()) + "X" + - std::to_string(outputImage.getLumaHeight()) + "_" + getVPIBackendString(m_backend) + "Backend_" + - GetMemoryTypeAsString(inputImage.isCPU()) + "Input"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - - return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); -} - -std::error_code VPITensorStream::ColorConvert(Image &outputImage, const Image &inputImage) -{ - std::unique_lock scopedLock{m_fence}; -#ifdef NVBENCH_ENABLE - std::string tag = "VPIColorConvert_BGR_NV12_" + std::to_string(inputImage.getLumaWidth()) + "X" + - std::to_string(inputImage.getLumaHeight()) + "_" + std::to_string(outputImage.getWidth()) + "X" + - std::to_string(outputImage.getHeight()) + "_" + getVPIBackendString(m_backend) + "Backend_" + - GetMemoryTypeAsString(inputImage.isCPU()) + "Input"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - - return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); -} - -std::error_code VPITensorStream::ColorConvert(Image &outputImage, const Image &inputImage) -{ - std::unique_lock scopedLock{m_fence}; -#ifdef NVBENCH_ENABLE - std::string tag = "VPIColorConvert_BGR_NV24_" + std::to_string(inputImage.getLumaWidth()) + "X" + - std::to_string(inputImage.getLumaHeight()) + "_" + std::to_string(outputImage.getWidth()) + "X" + - std::to_string(outputImage.getHeight()) + "_" + getVPIBackendString(m_backend) + "Backend_" + - GetMemoryTypeAsString(inputImage.isCPU()) + "Input"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - - return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); -} - -std::error_code VPITensorStream::ColorConvert(Image &outputImage, const Image &inputImage) -{ - std::unique_lock scopedLock{m_fence}; -#ifdef NVBENCH_ENABLE - std::string tag = "VPIColorConvert_NV12_RGB_" + std::to_string(inputImage.getWidth()) + "X" + - std::to_string(inputImage.getHeight()) + "_" + std::to_string(outputImage.getLumaWidth()) + "X" + - std::to_string(outputImage.getLumaHeight()) + "_" + getVPIBackendString(m_backend) + "Backend_" + - GetMemoryTypeAsString(inputImage.isCPU()) + "Input"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - - return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); -} - -std::error_code VPITensorStream::ColorConvert(Image &outputImage, const Image &inputImage) -{ - std::unique_lock scopedLock{m_fence}; -#ifdef NVBENCH_ENABLE - std::string tag = "VPIColorConvert_NV24_RGB_" + std::to_string(inputImage.getWidth()) + "X" + - std::to_string(inputImage.getHeight()) + "_" + std::to_string(outputImage.getLumaWidth()) + "X" + - std::to_string(outputImage.getLumaHeight()) + "_" + getVPIBackendString(m_backend) + "Backend_" + - GetMemoryTypeAsString(inputImage.isCPU()) + "Input"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - - return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); -} - -std::error_code VPITensorStream::ColorConvert(Image &outputImage, const Image &inputImage) -{ - std::unique_lock scopedLock{m_fence}; -#ifdef NVBENCH_ENABLE - std::string tag = "VPIColorConvert_RGB_NV12_" + std::to_string(inputImage.getLumaWidth()) + "X" + - std::to_string(inputImage.getLumaHeight()) + "_" + std::to_string(outputImage.getWidth()) + "X" + - std::to_string(outputImage.getHeight()) + "_" + getVPIBackendString(m_backend) + "Backend_" + - GetMemoryTypeAsString(inputImage.isCPU()) + "Input"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - - return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); -} - -std::error_code VPITensorStream::ColorConvert(Image &outputImage, const Image &inputImage) -{ - std::unique_lock scopedLock{m_fence}; -#ifdef NVBENCH_ENABLE - std::string tag = "VPIColorConvert_RGB_NV24_" + std::to_string(inputImage.getLumaWidth()) + "X" + - std::to_string(inputImage.getLumaHeight()) + "_" + std::to_string(outputImage.getWidth()) + "X" + - std::to_string(outputImage.getHeight()) + "_" + getVPIBackendString(m_backend) + "Backend_" + - GetMemoryTypeAsString(inputImage.isCPU()) + "Input"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - - return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); -} - -std::error_code VPITensorStream::StereoDisparityEstimator(Image &outputImage, const Image &inputLeftImage, - const Image &inputRightImage, size_t windowSize, - size_t maxDisparity) -{ - std::unique_lock scopedLock{m_fence}; -#ifdef NVBENCH_ENABLE - std::string tag = "StereoDisparityEstimator_Y_F32_Y_U8_" + std::to_string(inputLeftImage.getWidth()) + "X" + - std::to_string(inputLeftImage.getHeight()) + "_" + std::to_string(outputImage.getWidth()) + "X" + - std::to_string(outputImage.getHeight()) + "_" + getVPIBackendString(m_backend) + "Backend_" + - GetMemoryTypeAsString(inputLeftImage.isCPU()) + "Input"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - return m_stereoDisparityEstimator->execute(outputImage, inputLeftImage, inputRightImage, windowSize, maxDisparity, - m_stream, m_backend); -} - -std::error_code VPITensorStream::StereoDisparityEstimator(Image &outputImage, const Image &inputLeftImage, - const Image &inputRightImage, size_t windowSize, - size_t maxDisparity) -{ - std::unique_lock scopedLock{m_fence}; -#ifdef NVBENCH_ENABLE - std::string tag = "StereoDisparityEstimator_Y_F32_NV12_" + std::to_string(inputLeftImage.getLumaWidth()) + "X" + - std::to_string(inputLeftImage.getLumaHeight()) + "_" + std::to_string(outputImage.getWidth()) + - "X" + std::to_string(outputImage.getHeight()) + "_" + getVPIBackendString(m_backend) + - "Backend_" + GetMemoryTypeAsString(inputLeftImage.isCPU()) + "Input"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - return m_stereoDisparityEstimator->execute(outputImage, inputLeftImage, inputRightImage, windowSize, maxDisparity, - m_stream, m_backend); -} - -std::error_code VPITensorStream::StereoDisparityEstimator(Image &outputImage, const Image &inputLeftImage, - const Image &inputRightImage, size_t windowSize, - size_t maxDisparity) -{ - std::unique_lock scopedLock{m_fence}; -#ifdef NVBENCH_ENABLE - std::string tag = "StereoDisparityEstimator_Y_F32_NV24_" + std::to_string(inputLeftImage.getLumaWidth()) + "X" + - std::to_string(inputLeftImage.getLumaHeight()) + "_" + std::to_string(outputImage.getWidth()) + - "X" + std::to_string(outputImage.getHeight()) + "_" + getVPIBackendString(m_backend) + - "Backend_" + GetMemoryTypeAsString(inputLeftImage.isCPU()) + "Input"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - return m_stereoDisparityEstimator->execute(outputImage, inputLeftImage, inputRightImage, windowSize, maxDisparity, - m_stream, m_backend); -} - -TensorBackend VPITensorContext::Backend() const noexcept -{ - return TensorBackend::VPI; -} - -std::error_code VPITensorStream::GenerateWarpFromCameraModel(ImageWarp &warp, const ImageGrid &grid, - const CameraModel &source, const CameraIntrinsics &target) -{ - std::unique_lock scopedLock{m_fence}; -#ifdef NVBENCH_ENABLE - std::string tag = "GenerateWarpFromCameraModel"; - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - - VPIStatus status = VPI_SUCCESS; - - VPIWarpMap map = {0}; - map.grid.numHorizRegions = grid.numHorizRegions; - for (std::size_t i = 0; i < static_cast(grid.numHorizRegions); i++) - { - map.grid.regionWidth[i] = grid.regionWidth[i]; - map.grid.horizInterval[i] = grid.horizInterval[i]; - } - map.grid.numVertRegions = grid.numVertRegions; - for (std::size_t i = 0; i < static_cast(grid.numVertRegions); i++) - { - map.grid.regionHeight[i] = grid.regionHeight[i]; - map.grid.vertInterval[i] = grid.vertInterval[i]; - } - status = vpiWarpMapAllocData(&map); - - if ((status == VPI_SUCCESS) && (map.keypoints)) - { - switch (source.distortion.type) - { - case CameraDistortionType::Polynomial: - { - VPIPolynomialLensDistortionModel distortion; - distortion.k1 = source.distortion.k1; - distortion.k2 = source.distortion.k2; - distortion.k3 = source.distortion.k3; - distortion.k4 = source.distortion.k4; - distortion.k5 = source.distortion.k5; - distortion.k6 = source.distortion.k6; - distortion.p1 = source.distortion.p1; - distortion.p2 = source.distortion.p2; - status = vpiWarpMapGenerateFromPolynomialLensDistortionModel( - source.intrinsic.m_intrinsics, source.extrinsic.m_extrinsics, target.m_intrinsics, &distortion, &map); - break; - } - case CameraDistortionType::FisheyeEquidistant: - { - VPIFisheyeLensDistortionModel distortion; - distortion.k1 = source.distortion.k1; - distortion.k2 = source.distortion.k2; - distortion.k3 = source.distortion.k3; - distortion.k4 = source.distortion.k4; - distortion.mapping = VPI_FISHEYE_EQUIDISTANT; - status = vpiWarpMapGenerateFromFisheyeLensDistortionModel( - source.intrinsic.m_intrinsics, source.extrinsic.m_extrinsics, target.m_intrinsics, &distortion, &map); - break; - } - case CameraDistortionType::FisheyeEquisolid: - { - VPIFisheyeLensDistortionModel distortion; - distortion.k1 = source.distortion.k1; - distortion.k2 = source.distortion.k2; - distortion.k3 = source.distortion.k3; - distortion.k4 = source.distortion.k4; - distortion.mapping = VPI_FISHEYE_EQUISOLID; - status = vpiWarpMapGenerateFromFisheyeLensDistortionModel( - source.intrinsic.m_intrinsics, source.extrinsic.m_extrinsics, target.m_intrinsics, &distortion, &map); - break; - } - case CameraDistortionType::FisheyeOrthoGraphic: - { - VPIFisheyeLensDistortionModel distortion; - distortion.k1 = source.distortion.k1; - distortion.k2 = source.distortion.k2; - distortion.k3 = source.distortion.k3; - distortion.k4 = source.distortion.k4; - distortion.mapping = VPI_FISHEYE_ORTHOGRAPHIC; - status = vpiWarpMapGenerateFromFisheyeLensDistortionModel( - source.intrinsic.m_intrinsics, source.extrinsic.m_extrinsics, target.m_intrinsics, &distortion, &map); - break; - } - case CameraDistortionType::FisheyeStereographic: - { - VPIFisheyeLensDistortionModel distortion; - distortion.k1 = source.distortion.k1; - distortion.k2 = source.distortion.k2; - distortion.k3 = source.distortion.k3; - distortion.k4 = source.distortion.k4; - distortion.mapping = VPI_FISHEYE_STEREOGRAPHIC; - status = vpiWarpMapGenerateFromFisheyeLensDistortionModel( - source.intrinsic.m_intrinsics, source.extrinsic.m_extrinsics, target.m_intrinsics, &distortion, &map); - break; - } - default: - status = VPI_ERROR_INVALID_ARGUMENT; - break; - } - } - - if ((status == VPI_SUCCESS) && (map.keypoints)) - { - if (warp != nullptr) - { - vpiPayloadDestroy(reinterpret_cast(warp)->payload); - delete warp; - } - warp = new VPIImageWarp; - status = vpiCreateRemap(m_backend, &map, &(reinterpret_cast(warp)->payload)); - } - - // Delete map after payload is generated - vpiWarpMapFreeData(&map); - - return make_error_code(status); -} - -std::error_code VPITensorStream::DestroyWarp(ImageWarp &warp) noexcept -{ - std::unique_lock scopedLock{m_fence}; - if (warp != nullptr) - { - try - { - vpiPayloadDestroy(reinterpret_cast(warp)->payload); - } - catch (std::error_code &e) - { - return e; - } - - delete reinterpret_cast(warp); - warp = nullptr; - } - return make_error_code(ErrorCode::SUCCESS); -} - -}} // namespace cvcore::tensor_ops diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPITensorOperators.h b/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPITensorOperators.h deleted file mode 100644 index e01ed11..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPITensorOperators.h +++ /dev/null @@ -1,272 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 - -#ifndef CVCORE_VPITENSOROPERATORS_H -#define CVCORE_VPITENSOROPERATORS_H - -#include -#include - -#include -#include -#include -#include - -#include "cv/core/CameraModel.h" -#include "cv/tensor_ops/ITensorOperatorContext.h" -#include "cv/tensor_ops/ITensorOperatorStream.h" - -#include "VPIEnumMapping.h" - -namespace cvcore { namespace tensor_ops { - -/** - * Returns the corresponding VPI backend given the cvcore compute engine. - * @param computeEngine Compute engine used. - * @return Returns the VPIbackend. - */ -VPIBackend getVPIBackend(const ComputeEngine &computeEngine); - -/** - * Wraps a CVCore Image type into a VPIImage - * @param vpiImage VPIImage - * @param imgdata VPIImage data - * @param cvcoreImage CVCore image - * @param backend Compute backend - * @return error code - */ -template -std::error_code CreateVPIImageWrapper(VPIImage &vpiImg, VPIImageData &imgdata, const Image &cvcoreImage, VPIBackend backend); - -/** - * Update VPI Image data pointer - * @param vpiImage VPIImage - * @param imgdata VPIImage data - * @param isCPU data is on CPU or GPU - * @return error code - */ -std::error_code UpdateVPIImageWrapper(VPIImage &image, VPIImageData &imageWrap, bool isCPU); - -/** - * Destory Wrapped VPI Image - * @param vpiImage VPIImage - * @param imgdata VPIImage data - * @return error code - */ -std::error_code DestroyVPIImageWrapper(VPIImage &image, VPIImageData &imageWrap); - -/** - * Update VPI Image given CVCORE Image - * @param vpiImage VPIImage - * @param vpiImageData VPIImage data - * @param image CVCORE Image - * @return error code - */ -template::value>::type * = nullptr> -std::error_code UpdateImage(VPIImage &vpiImage, VPIImageData &vpiImageData, const Image &image) -{ - using D = typename Image::DataType; - vpiImageData.buffer.pitch.planes[0].data = const_cast(image.getData()); - return UpdateVPIImageWrapper(vpiImage, vpiImageData, image.isCPU()); -} - -/** - * Update VPI Image given CVCORE Image - * @param vpiImage VPIImage - * @param vpiImageData VPIImage data - * @param image CVCORE Image - * @return error code - */ -template::value>::type * = nullptr> -std::error_code UpdateImage(VPIImage &vpiImage, VPIImageData &vpiImageData, const Image &image) -{ - vpiImageData.buffer.pitch.planes[0].data = const_cast(image.getLumaData()); - vpiImageData.buffer.pitch.planes[1].data = const_cast(image.getChromaData()); - return UpdateVPIImageWrapper(vpiImage, vpiImageData, image.isCPU()); -} - -/** - * Check if params of VPIImageData is consistent with the given CVCORE Image - * @param vpiImageData VPIImage data - * @param image CVCORE Image - * @return whether param changed - */ -template::value && !IsPlanarImage::value>::type * = nullptr> -bool CheckParamsChanged(VPIImageData &vpiImageData, const Image &image) -{ - bool paramsChanged = false; - // Did format change - paramsChanged = paramsChanged || vpiImageData.buffer.pitch.format != ToVpiImageFormat(T); - // Did image dimension change - paramsChanged = - paramsChanged || (vpiImageData.buffer.pitch.planes[0].height != static_cast(image.getHeight()) || - vpiImageData.buffer.pitch.planes[0].width != static_cast(image.getWidth())); - return paramsChanged; -} - -/** - * Check if params of VPIImageData is consistent with the given CVCORE Image - * @param vpiImageData VPIImage data - * @param image CVCORE Image - * @return whether param changed - */ -template::value>::type * = nullptr> -bool CheckParamsChanged(VPIImageData &vpiImageData, const Image &image) -{ - bool paramsChanged = false; - - // Did format change - paramsChanged = paramsChanged || vpiImageData.buffer.pitch.format != ToVpiImageFormat(T); - - // Did image dimension change - paramsChanged = paramsChanged || - (vpiImageData.buffer.pitch.planes[0].height != static_cast(image.getLumaHeight()) || - vpiImageData.buffer.pitch.planes[0].width != static_cast(image.getLumaWidth()) || - vpiImageData.buffer.pitch.planes[1].height != static_cast(image.getChromaHeight()) || - vpiImageData.buffer.pitch.planes[1].width != static_cast(image.getChromaWidth())); - return paramsChanged; -} - -/** - * Implementation of VPITensorContext - */ -class VPITensorContext : public ITensorOperatorContext -{ -public: - /** - * Default Constructor for VPI Context. - */ - VPITensorContext() = default; - - /** - * Default Destructor for VPI Context. - */ - ~VPITensorContext() = default; - - /** - * Creates a stream based on compute engine - * @param computeEngine CVCore Compute engine - * @return Pointer to ITensorOperatorStream object. - */ - virtual std::error_code CreateStream(TensorOperatorStream &, const ComputeEngine &computeEngine) override; - - /** - * Destroy stream creates. - * @param inputStream Input stream to be deleted - * @return Error code - */ - virtual std::error_code DestroyStream(TensorOperatorStream &inputStream) override; - - /** - * Checks if stream type is supported for a given backend. - * @param computeEngine CVCore Compute engine - * @return true if stream type is available. - */ - virtual bool IsComputeEngineCompatible(const ComputeEngine &computeEngine) const noexcept override; - - /** - * Returns the backend type - */ - virtual TensorBackend Backend() const noexcept override; - -private: -}; - -/** - * Implementation of VPITensorStream - */ -class VPITensorStream : public ITensorOperatorStream -{ -public: - virtual std::error_code Status() noexcept override; - - virtual std::error_code GenerateWarpFromCameraModel(ImageWarp &warp, const ImageGrid &grid, - const CameraModel &source, - const CameraIntrinsics &target) override; - virtual std::error_code DestroyWarp(ImageWarp &warp) noexcept override; - - virtual std::error_code Remap(Image &outputImage, const Image &inputImage, const ImageWarp warp, - InterpolationType interpolation, BorderType border) override; - - virtual std::error_code Remap(Image &outputImage, const Image &inputImage, const ImageWarp warp, - InterpolationType interpolation, BorderType border) override; - - virtual std::error_code Remap(Image &outputImage, const Image &inputImage, const ImageWarp warp, - InterpolationType interpolation, BorderType border) override; - - virtual std::error_code Remap(Image &outputImage, const Image &inputImage, const ImageWarp warp, - InterpolationType interpolation, BorderType border) override; - - virtual std::error_code Resize(Image &outputImage, const Image &inputImage, - InterpolationType interpolation, BorderType border) override; - virtual std::error_code Resize(Image &outputImage, const Image &inputImage, - InterpolationType interpolation, BorderType border) override; - virtual std::error_code Resize(Image &outputImage, const Image &inputImage, - InterpolationType interpolation, BorderType border) override; - virtual std::error_code Resize(Image &outputImage, const Image &inputImage, - InterpolationType interpolation, BorderType border) override; - virtual std::error_code Resize(Image &outputImage, const Image &inputImage, - InterpolationType interpolation, BorderType border) override; - - virtual std::error_code ColorConvert(Image &outputImage, const Image &inputImage) override; - virtual std::error_code ColorConvert(Image &outputImage, const Image &inputImage) override; - virtual std::error_code ColorConvert(Image &outputImage, const Image &inputImage) override; - virtual std::error_code ColorConvert(Image &outputImage, const Image &inputImage) override; - virtual std::error_code ColorConvert(Image &outputImage, const Image &inputImage) override; - virtual std::error_code ColorConvert(Image &outputImage, const Image &inputImage) override; - virtual std::error_code ColorConvert(Image &outputImage, const Image &inputImage) override; - virtual std::error_code ColorConvert(Image &outputImage, const Image &inputImage) override; - virtual std::error_code ColorConvert(Image &outputImage, const Image &inputImage) override; - virtual std::error_code ColorConvert(Image &outputImage, const Image &inputImage) override; - virtual std::error_code StereoDisparityEstimator(Image &outputImage, const Image &inputLeftImage, - const Image &inputRightImage, size_t windowSize, - size_t maxDisparity) override; - virtual std::error_code StereoDisparityEstimator(Image &outputImage, const Image &inputLeftImage, - const Image &inputRightImage, size_t windowSize, - size_t maxDisparity) override; - virtual std::error_code StereoDisparityEstimator(Image &outputImage, const Image &inputLeftImage, - const Image &inputRightImage, size_t windowSize, - size_t maxDisparity) override; - -protected: - friend class VPITensorContext; - - VPITensorStream(const ComputeEngine &computeEngine); - ~VPITensorStream(); - -private: - class VPIResizeImpl; - class VPIRemapImpl; - class VPIColorConvertImpl; - class VPIStereoDisparityEstimatorImpl; - - mutable std::mutex m_fence; - - std::unique_ptr m_resizer; - std::unique_ptr m_remapper; - std::unique_ptr m_colorConverter; - std::unique_ptr m_stereoDisparityEstimator; - - VPIStream m_stream; - VPIBackend m_backend; -}; - -}} // namespace cvcore::tensor_ops - -#endif // CVCORE_VPITENSOROPERATORS_H diff --git a/isaac_ros_image_proc/gxf/tensorops/detail/ImageAdapterVideoBufferImpl.hpp b/isaac_ros_image_proc/gxf/tensorops/detail/ImageAdapterVideoBufferImpl.hpp deleted file mode 100644 index 6664640..0000000 --- a/isaac_ros_image_proc/gxf/tensorops/detail/ImageAdapterVideoBufferImpl.hpp +++ /dev/null @@ -1,294 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 -#ifndef NVIDIA_CVCORE_VIDEO_BUFFER_ADAPTER_HPP -#define NVIDIA_CVCORE_VIDEO_BUFFER_ADAPTER_HPP - -#include "../ImageUtils.hpp" - -#include "gxf/multimedia/video.hpp" -#include "gxf/std/allocator.hpp" - -#include "cv/core/Image.h" - -namespace nvidia { -namespace cvcore { -namespace tensor_ops { -namespace detail { - -gxf::Expected<::cvcore::ImageType> GetImageTypeFromVideoFormat(const gxf::VideoFormat format); - -gxf::Expected GetVideoBufferInfo(gxf::Handle video_buffer); - -template<::cvcore::ImageType T, - typename std::enable_if::type* = nullptr> -gxf::Expected<::cvcore::Image> WrapImageFromVideoBuffer(gxf::Handle video_buffer) { - const auto info = GetVideoBufferInfo(video_buffer); - if (!info) { - return gxf::Unexpected{GXF_FAILURE}; - } - using D = typename ::cvcore::detail::ChannelTypeToNative<::cvcore::ImageTraits::CT>::Type; - auto pointer = reinterpret_cast(video_buffer->pointer()); - if (!pointer) { - return gxf::Unexpected{GXF_FAILURE}; - } - const auto& color_planes = video_buffer->video_frame_info().color_planes; - return ::cvcore::Image(info.value().width, info.value().height, color_planes[0].stride, pointer, - info.value().is_cpu); -} - -template<::cvcore::ImageType T, - typename std::enable_if::type* = nullptr> -gxf::Expected<::cvcore::Image> WrapImageFromVideoBuffer(gxf::Handle video_buffer) { - const auto info = GetVideoBufferInfo(video_buffer); - if (!info) { - return gxf::Unexpected{GXF_FAILURE}; - } - // Note only U8 is supported in NV12/NV24 - auto pointer = reinterpret_cast(video_buffer->pointer()); - if (!pointer) { - return gxf::Unexpected{GXF_FAILURE}; - } - const auto& color_planes = video_buffer->video_frame_info().color_planes; - return ::cvcore::Image(info.value().width, info.value().height, color_planes[0].stride, color_planes[1].stride, - pointer, pointer + color_planes[1].offset, info.value().is_cpu); -} - -template<::cvcore::ImageType T> -struct ImageTypeToVideoFormat { - static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_CUSTOM; -}; - -template<> -struct ImageTypeToVideoFormat<::cvcore::ImageType::NV12> { - static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_NV12_ER; -}; - -template<> -struct ImageTypeToVideoFormat<::cvcore::ImageType::NV24> { - static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_NV24_ER; -}; - -template<> -struct ImageTypeToVideoFormat<::cvcore::ImageType::RGBA_U8> { - static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_RGBA; -}; - -template<> -struct ImageTypeToVideoFormat<::cvcore::ImageType::RGB_U8> { - static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_RGB; -}; - -template<> -struct ImageTypeToVideoFormat<::cvcore::ImageType::RGB_F32> { - static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_RGB32; -}; - -template<> -struct ImageTypeToVideoFormat<::cvcore::ImageType::BGR_U8> { - static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_BGR; -}; - -template<> -struct ImageTypeToVideoFormat<::cvcore::ImageType::BGR_F32> { - static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_BGR32; -}; - -template<> -struct ImageTypeToVideoFormat<::cvcore::ImageType::PLANAR_RGB_U8> { - static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_R8_G8_B8; -}; - -template<> -struct ImageTypeToVideoFormat<::cvcore::ImageType::PLANAR_RGB_F32> { - static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_R32_G32_B32; -}; - -template<> -struct ImageTypeToVideoFormat<::cvcore::ImageType::PLANAR_BGR_U8> { - static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_B8_G8_R8; -}; - -template<> -struct ImageTypeToVideoFormat<::cvcore::ImageType::PLANAR_BGR_F32> { - static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_B32_G32_R32; -}; - -template<> -struct ImageTypeToVideoFormat<::cvcore::ImageType::Y_U8> { - static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY; -}; - -template<> -struct ImageTypeToVideoFormat<::cvcore::ImageType::Y_F32> { - static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY32; -}; - -template<::cvcore::ImageType T> -struct DefaultNoPaddingColorPlanes {}; - -template<> -struct DefaultNoPaddingColorPlanes<::cvcore::ImageType::NV12> { - DefaultNoPaddingColorPlanes(size_t width) - : planes({gxf::ColorPlane("Y", 1, width), gxf::ColorPlane("UV", 2, width)}) {} - std::array planes; -}; - -template<> -struct DefaultNoPaddingColorPlanes<::cvcore::ImageType::NV24> { - DefaultNoPaddingColorPlanes(size_t width) - : planes({gxf::ColorPlane("Y", 1, width), gxf::ColorPlane("UV", 2, width * 2)}) {} - std::array planes; -}; - -template<> -struct DefaultNoPaddingColorPlanes<::cvcore::ImageType::RGBA_U8> { - DefaultNoPaddingColorPlanes(size_t width) - : planes({gxf::ColorPlane("RGBA", 4, width * 4)}) {} - std::array planes; -}; - -template<> -struct DefaultNoPaddingColorPlanes<::cvcore::ImageType::RGB_U8> { - DefaultNoPaddingColorPlanes(size_t width) - : planes({gxf::ColorPlane("RGB", 3, width * 3)}) {} - std::array planes; -}; - -template<> -struct DefaultNoPaddingColorPlanes<::cvcore::ImageType::RGB_F32> { - DefaultNoPaddingColorPlanes(size_t width) - : planes({gxf::ColorPlane("RGB", 12, width * 12)}) {} - std::array planes; -}; - -template<> -struct DefaultNoPaddingColorPlanes<::cvcore::ImageType::BGR_U8> { - DefaultNoPaddingColorPlanes(size_t width) - : planes({gxf::ColorPlane("BGR", 3, width * 3)}) {} - std::array planes; -}; - -template<> -struct DefaultNoPaddingColorPlanes<::cvcore::ImageType::BGR_F32> { - DefaultNoPaddingColorPlanes(size_t width) - : planes({gxf::ColorPlane("BGR", 12, width * 12)}) {} - std::array planes; -}; - -template<> -struct DefaultNoPaddingColorPlanes<::cvcore::ImageType::PLANAR_RGB_U8> { - DefaultNoPaddingColorPlanes(size_t width) - : planes({gxf::ColorPlane("R", 1, width), gxf::ColorPlane("G", 1, width), gxf::ColorPlane("B", 1, width)}) {} - std::array planes; -}; - -template<> -struct DefaultNoPaddingColorPlanes<::cvcore::ImageType::PLANAR_RGB_F32> { - DefaultNoPaddingColorPlanes(size_t width) - : planes( - {gxf::ColorPlane("R", 4, width * 4), gxf::ColorPlane("G", 4, width * 4), gxf::ColorPlane("B", 4, width * 4)}) {} - std::array planes; -}; - -template<> -struct DefaultNoPaddingColorPlanes<::cvcore::ImageType::PLANAR_BGR_U8> { - DefaultNoPaddingColorPlanes(size_t width) - : planes({gxf::ColorPlane("B", 1, width), gxf::ColorPlane("G", 1, width), gxf::ColorPlane("R", 1, width)}) {} - std::array planes; -}; - -template<> -struct DefaultNoPaddingColorPlanes<::cvcore::ImageType::PLANAR_BGR_F32> { - DefaultNoPaddingColorPlanes(size_t width) - : planes( - {gxf::ColorPlane("B", 4, width * 4), gxf::ColorPlane("G", 4, width * 4), gxf::ColorPlane("R", 4, width * 4)}) {} - std::array planes; -}; - -template<> -struct DefaultNoPaddingColorPlanes<::cvcore::ImageType::Y_U8> { - DefaultNoPaddingColorPlanes(size_t width) - : planes({gxf::ColorPlane("gray", 1, width)}) {} - std::array planes; -}; - -template<> -struct DefaultNoPaddingColorPlanes<::cvcore::ImageType::Y_F32> { - DefaultNoPaddingColorPlanes(size_t width) - : planes({gxf::ColorPlane("gray", 4, width * 4)}) {} - std::array planes; -}; - -// This include the list of image types that GXF supported so far -constexpr bool IsValidGXFImageType(const ::cvcore::ImageType type) { - return type == ::cvcore::ImageType::NV12 || type == ::cvcore::ImageType::NV24 || - type == ::cvcore::ImageType::RGBA_U8 || type == ::cvcore::ImageType::RGB_U8 || - type == ::cvcore::ImageType::BGR_U8 || type == ::cvcore::ImageType::RGB_F32 || - type == ::cvcore::ImageType::BGR_F32 || type == ::cvcore::ImageType::PLANAR_RGB_U8 || - type == ::cvcore::ImageType::PLANAR_BGR_U8 || type == ::cvcore::ImageType::PLANAR_RGB_F32 || - type == ::cvcore::ImageType::PLANAR_BGR_F32 || type == ::cvcore::ImageType::Y_U8 || - type == ::cvcore::ImageType::Y_F32; -} - -template<::cvcore::ImageType T, typename std::enable_if::type* = nullptr> -gxf_result_t AllocateVideoBuffer(gxf::Handle video_buffer, size_t width, size_t height, - gxf::Handle allocator, bool is_cpu, bool allocate_pitch_linear) { - if (width % 2 != 0 || height % 2 != 0) { - GXF_LOG_ERROR("image width/height must be even for creation of gxf::VideoBuffer"); - return GXF_FAILURE; - } - if (allocate_pitch_linear) { - auto result = video_buffer->resize::format>( - static_cast(width), static_cast(height), gxf::SurfaceLayout::GXF_SURFACE_LAYOUT_PITCH_LINEAR, - is_cpu ? gxf::MemoryStorageType::kHost : gxf::MemoryStorageType::kDevice, allocator); - - if (!result) { - GXF_LOG_ERROR("resize VideoBuffer failed."); - return GXF_FAILURE; - } - } else { - DefaultNoPaddingColorPlanes planes_trait(width); - gxf::VideoFormatSize::format> buffer_type_trait; - uint64_t size = buffer_type_trait.size(width, height, planes_trait.planes); - std::vector planes_filled{planes_trait.planes.begin(), planes_trait.planes.end()}; - gxf::VideoBufferInfo buffer_info{static_cast(width), static_cast(height), - ImageTypeToVideoFormat::format, planes_filled, - gxf::SurfaceLayout::GXF_SURFACE_LAYOUT_PITCH_LINEAR}; - auto result = video_buffer->resizeCustom( - buffer_info, size, is_cpu ? gxf::MemoryStorageType::kHost : gxf::MemoryStorageType::kDevice, allocator); - - if (!result) { - GXF_LOG_ERROR("custom resize VideoBuffer failed."); - return GXF_FAILURE; - } - } - return GXF_SUCCESS; -} - -template<::cvcore::ImageType T, typename std::enable_if::type* = nullptr> -gxf_result_t AllocateVideoBuffer(gxf::Handle video_buffer, size_t width, size_t height, - gxf::Handle allocator, bool is_cpu, bool allocate_pitch_linear) { - GXF_LOG_ERROR("image type not supported in gxf::VideoBuffer"); - return GXF_FAILURE; -} - -} // namespace detail -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia - -#endif diff --git a/isaac_ros_image_proc/gxf/tensorops/CameraModel.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/CameraModel.cpp similarity index 73% rename from isaac_ros_image_proc/gxf/tensorops/CameraModel.cpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/CameraModel.cpp index 5dce98e..935bfe4 100644 --- a/isaac_ros_image_proc/gxf/tensorops/CameraModel.cpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/CameraModel.cpp @@ -16,32 +16,34 @@ // SPDX-License-Identifier: Apache-2.0 #include "CameraModel.hpp" +#include namespace nvidia { -namespace cvcore { +namespace isaac { namespace tensor_ops { namespace detail { -gxf::Expected<::cvcore::CameraDistortionType> GetCameraDistortionType(const std::string& type) { +gxf::Expected GetCameraDistortionType( + const std::string& type) { if (type == "Perspective") { - return ::cvcore::CameraDistortionType::NONE; + return cvcore::tensor_ops::CameraDistortionType::NONE; } else if (type == "Polynomial") { - return ::cvcore::CameraDistortionType::Polynomial; + return cvcore::tensor_ops::CameraDistortionType::Polynomial; } else if (type == "FisheyeEquidistant") { - return ::cvcore::CameraDistortionType::FisheyeEquidistant; + return cvcore::tensor_ops::CameraDistortionType::FisheyeEquidistant; } else if (type == "FisheyeEquisolid") { - return ::cvcore::CameraDistortionType::FisheyeEquisolid; + return cvcore::tensor_ops::CameraDistortionType::FisheyeEquisolid; } else if (type == "FisheyeOrthoGraphic") { - return ::cvcore::CameraDistortionType::FisheyeOrthoGraphic; + return cvcore::tensor_ops::CameraDistortionType::FisheyeOrthoGraphic; } else if (type == "FisheyeStereographic") { - return ::cvcore::CameraDistortionType::FisheyeStereographic; + return cvcore::tensor_ops::CameraDistortionType::FisheyeStereographic; } else { return gxf::Unexpected{GXF_FAILURE}; } } -} // namespace detail +} // namespace detail gxf_result_t CameraModel::registerInterface(gxf::Registrar* registrar) { gxf::Expected result; @@ -75,12 +77,14 @@ gxf_result_t CameraModel::initialize() { GXF_LOG_ERROR("focal length and principle point must be 2-element array."); return GXF_FAILURE; } - intrinsics_ = ::cvcore::CameraIntrinsics(focal_length_.get()[0], focal_length_.get()[1], principle_point_.get()[0], - principle_point_.get()[1], skew_value_.get()); + intrinsics_ = cvcore::tensor_ops::CameraIntrinsics( + focal_length_.get()[0], focal_length_.get()[1], + principle_point_.get()[0], principle_point_.get()[1], + skew_value_.get()); return GXF_SUCCESS; } -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/CameraModel.hpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/CameraModel.hpp similarity index 75% rename from isaac_ros_image_proc/gxf/tensorops/CameraModel.hpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/CameraModel.hpp index e68c67f..6843706 100644 --- a/isaac_ros_image_proc/gxf/tensorops/CameraModel.hpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/CameraModel.hpp @@ -14,47 +14,45 @@ // limitations under the License. // // SPDX-License-Identifier: Apache-2.0 -#ifndef NVIDIA_CVCORE_CAMERA_MODEL_HPP -#define NVIDIA_CVCORE_CAMERA_MODEL_HPP +#pragma once +#include "extensions/tensorops/core/CameraModel.h" +#include +#include #include "gxf/core/component.hpp" #include "gxf/std/parameter_parser_std.hpp" -#include "cv/core/CameraModel.h" - namespace nvidia { -namespace cvcore { +namespace isaac { namespace tensor_ops { // Wrapper of CameraModel compatible with CVCORE class CameraModel : public gxf::Component { -public: + public: virtual ~CameraModel() = default; CameraModel() = default; gxf_result_t registerInterface(gxf::Registrar* registrar) override; gxf_result_t initialize() override; - ::cvcore::CameraDistortionModel getDistortionModel() const { + cvcore::tensor_ops::CameraDistortionModel getDistortionModel() const { return distortions_; } - ::cvcore::CameraIntrinsics getCameraIntrinsics() const { + cvcore::tensor_ops::CameraIntrinsics getCameraIntrinsics() const { return intrinsics_; } -private: + private: gxf::Parameter distortion_type_; gxf::Parameter> distortion_coefficients_; gxf::Parameter> focal_length_; gxf::Parameter> principle_point_; gxf::Parameter skew_value_; - ::cvcore::CameraDistortionModel distortions_; - ::cvcore::CameraIntrinsics intrinsics_; + cvcore::tensor_ops::CameraDistortionModel distortions_; + cvcore::tensor_ops::CameraIntrinsics intrinsics_; }; -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia - -#endif +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/ConvertColorFormat.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/ConvertColorFormat.cpp new file mode 100644 index 0000000..9ff8af2 --- /dev/null +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/ConvertColorFormat.cpp @@ -0,0 +1,282 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 +#include "ConvertColorFormat.hpp" + +namespace nvidia { +namespace isaac { +namespace tensor_ops { + +namespace detail { + +template +gxf_result_t ConvertColorFormatImpl(gxf::Entity& output, gxf::Entity& input, + const ImageInfo& output_info, + const ImageInfo& input_info, const char* output_name, const char* input_name, + gxf::Handle output_adapter, gxf::Handle input_adapter, + gxf::Handle allocator, + cvcore::tensor_ops::ColorConversionType type, cudaStream_t stream) { + auto input_image = input_adapter->WrapImageFromMessage(input, input_name); + if (!input_image) { + return GXF_FAILURE; + } + + auto error = output_adapter->AddImageToMessage( + output, output_info.width, output_info.height, allocator, + output_info.is_cpu, output_name); + if (error != GXF_SUCCESS) { + return GXF_FAILURE; + } + + auto output_image = output_adapter->WrapImageFromMessage(output, output_name); + if (!output_image) { + return GXF_FAILURE; + } + + if (!cvcore::tensor_ops::ConvertColorFormat(output_image.value(), + input_image.value(), type, stream)) { + return GXF_FAILURE; + } + return GXF_SUCCESS; +} + +template +gxf_result_t ConvertColorFormatStreamImpl( + gxf::Entity& output, gxf::Entity& input, const ImageInfo& output_info, + const ImageInfo& input_info, const char* output_name, const char* input_name, + gxf::Handle stream, gxf::Handle output_adapter, + gxf::Handle input_adapter, + gxf::Handle allocator) { + auto input_image = input_adapter->WrapImageFromMessage(input, input_name); + if (!input_image) { + return GXF_FAILURE; + } + + auto error = output_adapter->AddImageToMessage( + output, output_info.width, output_info.height, allocator, + output_info.is_cpu, output_name); + if (error != GXF_SUCCESS) { + return GXF_FAILURE; + } + + auto output_image = output_adapter->WrapImageFromMessage(output, output_name); + if (!output_image) { + return GXF_FAILURE; + } + + auto err_code = stream->getStream()->ColorConvert(output_image.value(), input_image.value()); + if (err_code != cvcore::tensor_ops::make_error_condition( + cvcore::tensor_ops::ErrorCode::SUCCESS)) { + GXF_LOG_ERROR("color conversion operation failed."); + return GXF_FAILURE; + } + + return GXF_SUCCESS; +} + +} // namespace detail + +template +gxf_result_t ConvertColorFormatBase::registerInterface( + gxf::Registrar* registrar) { + gxf::Expected result; + + result &= registrar->parameter( + output_type_, "output_type"); + result &= registrar->parameter( + receiver_, "receiver"); + result &= registrar->parameter( + transmitter_, "transmitter"); + result &= registrar->parameter( + pool_, "pool"); + result &= registrar->parameter( + stream_, "stream", + "tensor stream", "tensor stream object", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + result &= registrar->parameter( + stream_pool_, "stream_pool", + "cuda stream pool", "cuda stream pool object", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + result &= registrar->parameter( + input_adapter_, "input_adapter"); + result &= registrar->parameter( + output_adapter_, "output_adapter"); + result &= registrar->parameter( + input_name_, "input_name", + "input name", "input tensor name", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + result &= registrar->parameter( + output_name_, "output_name", + "output name", "output tensor name", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + + return gxf::ToResultCode(result); +} + +template +gxf::Expected ConvertColorFormatBase::doInferOutputInfo( + gxf::Entity& input) { + // Set output type + auto output_type = GetImageTypeFromString(output_type_); + if (!output_type) { + return gxf::Unexpected{GXF_FAILURE}; + } + // Check if no-op is needed + no_op_ = output_type.value() == input_info_.type; + return ImageInfo{output_type.value(), input_info_.width, input_info_.height, input_info_.is_cpu}; +} + +template +gxf_result_t ConvertColorFormatBase::doUpdateCameraMessage( + gxf::Handle& output, + gxf::Handle& input) { + *output = *input; + return GXF_SUCCESS; +} + +#define DEFINE_CONVERT_COLOR_FORMAT(INPUT_TYPE, OUTPUT_TYPE, CONVERSION_TYPE) \ + if (input_info_.type == INPUT_TYPE && output_info_.type == OUTPUT_TYPE) { \ + return detail::ConvertColorFormatImpl( \ + output, input, output_info_, input_info_, output_name, input_name, \ + output_adapter_.get(), input_adapter_.get(), \ + pool_.get(), CONVERSION_TYPE, stream); \ + } + +#define DEFINE_STREAM_CONVERT_COLOR_FORMAT(INPUT_TYPE, OUTPUT_TYPE) \ + if (input_info_.type == INPUT_TYPE && output_info_.type == OUTPUT_TYPE) { \ + return detail::ConvertColorFormatStreamImpl( \ + output, input, output_info_, input_info_, output_name, input_name, \ + stream_.try_get().value(), \ + output_adapter_.get(), input_adapter_.get(), pool_.get()); \ + } + +template <> +gxf_result_t ConvertColorFormatBase::doExecute( + gxf::Entity& output, gxf::Entity& input, cudaStream_t stream, const char* output_name, + const char* input_name) { + // Run the color conversion operation + DEFINE_STREAM_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::RGB_U8, + cvcore::tensor_ops::ImageType::BGR_U8); + DEFINE_STREAM_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::BGR_U8, + cvcore::tensor_ops::ImageType::RGB_U8); + DEFINE_STREAM_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::NV12, + cvcore::tensor_ops::ImageType::BGR_U8); + DEFINE_STREAM_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::BGR_U8, + cvcore::tensor_ops::ImageType::NV12); + DEFINE_STREAM_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::NV12, + cvcore::tensor_ops::ImageType::RGB_U8); + DEFINE_STREAM_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::RGB_U8, + cvcore::tensor_ops::ImageType::NV12); + DEFINE_STREAM_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::NV24, + cvcore::tensor_ops::ImageType::BGR_U8); + DEFINE_STREAM_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::BGR_U8, + cvcore::tensor_ops::ImageType::NV24); + DEFINE_STREAM_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::NV24, + cvcore::tensor_ops::ImageType::RGB_U8); + DEFINE_STREAM_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::RGB_U8, + cvcore::tensor_ops::ImageType::NV24); + DEFINE_STREAM_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::BGR_U8, + cvcore::tensor_ops::ImageType::Y_U8); + DEFINE_STREAM_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::Y_U8, + cvcore::tensor_ops::ImageType::BGR_U8); + DEFINE_STREAM_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::RGB_U8, + cvcore::tensor_ops::ImageType::Y_U8); + DEFINE_STREAM_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::Y_U8, + cvcore::tensor_ops::ImageType::RGB_U8); + DEFINE_STREAM_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::NV12, + cvcore::tensor_ops::ImageType::Y_U8); + DEFINE_STREAM_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::Y_U8, + cvcore::tensor_ops::ImageType::NV12); + DEFINE_STREAM_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::NV24, + cvcore::tensor_ops::ImageType::Y_U8); + DEFINE_STREAM_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::Y_U8, + cvcore::tensor_ops::ImageType::NV24); + // Return error code for unsupported type + GXF_LOG_ERROR("invalid input/output type for image color conversion."); + return GXF_FAILURE; +} + +template <> +gxf_result_t ConvertColorFormatBase::doExecute( + gxf::Entity& output, gxf::Entity& input, cudaStream_t stream, const char* output_name, + const char* input_name) { + // Run the color conversion operation + DEFINE_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::RGB_U8, + cvcore::tensor_ops::ImageType::BGR_U8, + cvcore::tensor_ops::ColorConversionType::RGB2BGR); + DEFINE_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::RGB_U16, + cvcore::tensor_ops::ImageType::BGR_U16, + cvcore::tensor_ops::ColorConversionType::RGB2BGR); + DEFINE_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::RGB_F32, + cvcore::tensor_ops::ImageType::BGR_F32, + cvcore::tensor_ops::ColorConversionType::RGB2BGR); + DEFINE_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::BGR_U8, + cvcore::tensor_ops::ImageType::RGB_U8, + cvcore::tensor_ops::ColorConversionType::BGR2RGB); + DEFINE_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::BGR_U16, + cvcore::tensor_ops::ImageType::RGB_U16, + cvcore::tensor_ops::ColorConversionType::BGR2RGB); + DEFINE_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::BGR_F32, + cvcore::tensor_ops::ImageType::RGB_F32, + cvcore::tensor_ops::ColorConversionType::BGR2RGB); + DEFINE_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::RGB_U8, + cvcore::tensor_ops::ImageType::Y_U8, + cvcore::tensor_ops::ColorConversionType::RGB2GRAY); + DEFINE_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::RGB_U16, + cvcore::tensor_ops::ImageType::Y_U16, + cvcore::tensor_ops::ColorConversionType::RGB2GRAY); + DEFINE_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::RGB_F32, + cvcore::tensor_ops::ImageType::Y_F32, + cvcore::tensor_ops::ColorConversionType::RGB2GRAY); + DEFINE_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::BGR_U8, + cvcore::tensor_ops::ImageType::Y_U8, + cvcore::tensor_ops::ColorConversionType::BGR2GRAY); + DEFINE_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::BGR_U16, + cvcore::tensor_ops::ImageType::Y_U16, + cvcore::tensor_ops::ColorConversionType::BGR2GRAY); + DEFINE_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::BGR_F32, + cvcore::tensor_ops::ImageType::Y_F32, + cvcore::tensor_ops::ColorConversionType::BGR2GRAY); + DEFINE_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::Y_U8, + cvcore::tensor_ops::ImageType::RGB_U8, + cvcore::tensor_ops::ColorConversionType::GRAY2RGB); + DEFINE_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::Y_U16, + cvcore::tensor_ops::ImageType::RGB_U16, + cvcore::tensor_ops::ColorConversionType::GRAY2RGB); + DEFINE_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::Y_F32, + cvcore::tensor_ops::ImageType::RGB_F32, + cvcore::tensor_ops::ColorConversionType::GRAY2RGB); + DEFINE_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::Y_U8, + cvcore::tensor_ops::ImageType::BGR_U8, + cvcore::tensor_ops::ColorConversionType::GRAY2BGR); + DEFINE_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::Y_U16, + cvcore::tensor_ops::ImageType::BGR_U16, + cvcore::tensor_ops::ColorConversionType::GRAY2BGR); + DEFINE_CONVERT_COLOR_FORMAT(cvcore::tensor_ops::ImageType::Y_F32, + cvcore::tensor_ops::ImageType::BGR_F32, + cvcore::tensor_ops::ColorConversionType::GRAY2BGR); + + // Return error code for unsupported type + GXF_LOG_ERROR("invalid input/output type for image color conversion."); + return GXF_FAILURE; +} + +template class ConvertColorFormatBase; +template class ConvertColorFormatBase; + +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/ConvertColorFormat.hpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/ConvertColorFormat.hpp similarity index 78% rename from isaac_ros_image_proc/gxf/tensorops/ConvertColorFormat.hpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/ConvertColorFormat.hpp index f78786b..dccdc2c 100644 --- a/isaac_ros_image_proc/gxf/tensorops/ConvertColorFormat.hpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/ConvertColorFormat.hpp @@ -14,29 +14,29 @@ // limitations under the License. // // SPDX-License-Identifier: Apache-2.0 -#ifndef NVIDIA_CVCORE_CONVERT_COLOR_FORMAT_HPP -#define NVIDIA_CVCORE_CONVERT_COLOR_FORMAT_HPP +#pragma once -#include "TensorOperator.hpp" +#include +#include "extensions/tensorops/components/TensorOperator.hpp" namespace nvidia { -namespace cvcore { +namespace isaac { namespace tensor_ops { // ConvertColorFormat operator. template class ConvertColorFormatBase : public TensorOperator { -public: + public: virtual ~ConvertColorFormatBase() {} - gxf_result_t registerInterface(gxf::Registrar* registrar) override final; + gxf_result_t registerInterface(gxf::Registrar* registrar) override; -private: - gxf::Expected doInferOutputInfo(gxf::Entity& input) override final; + private: + gxf::Expected doInferOutputInfo(gxf::Entity& input) override; gxf_result_t doUpdateCameraMessage(gxf::Handle& output, - gxf::Handle& input) override final; - gxf_result_t doExecute(gxf::Entity& output, gxf::Entity& input, cudaStream_t stream, const char* output_name, - const char* input_name) override final; + gxf::Handle& input) override; + gxf_result_t doExecute(gxf::Entity& output, gxf::Entity& input, cudaStream_t stream, + const char* output_name, const char* input_name) override; gxf::Parameter output_type_; }; @@ -44,8 +44,6 @@ class ConvertColorFormatBase : public TensorOperator { class ConvertColorFormat : public ConvertColorFormatBase {}; class StreamConvertColorFormat : public ConvertColorFormatBase {}; -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia - -#endif +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/CropAndResize.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/CropAndResize.cpp new file mode 100644 index 0000000..498cc3e --- /dev/null +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/CropAndResize.cpp @@ -0,0 +1,189 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 + +#include "CropAndResize.hpp" + +#include +#include +#include +#include "extensions/tensorops/components/Resize.hpp" + +namespace nvidia { +namespace isaac { +namespace tensor_ops { + +namespace detail { + +template +gxf_result_t CropAndResizeImpl(gxf::Entity& output, + gxf::Entity& input, const ImageInfo& output_info, + const ImageInfo& input_info, const char* output_name, const char* input_name, + gxf::Handle output_adapter, gxf::Handle input_adapter, + gxf::Handle allocator, const std::vector& src_rois, + cvcore::tensor_ops::InterpolationType interp_type, cudaStream_t stream) { + auto input_image = input_adapter->WrapImageFromMessage(input, input_name); + if (!input_image) { + return GXF_FAILURE; + } + + const size_t num_output = src_rois.size(); + + for (size_t i = 0; i < num_output; i++) { + const std::string output_name_i = std::string(output_name) + "_" + std::to_string(i); + auto error = output_adapter->AddImageToMessage( + output, output_info.width, output_info.height, allocator, + output_info.is_cpu, output_name_i.c_str()); + if (error != GXF_SUCCESS) { + return GXF_FAILURE; + } + auto output_image = output_adapter->WrapImageFromMessage(output, output_name_i.c_str()); + if (!output_image) { + return GXF_FAILURE; + } + if (!cvcore::tensor_ops::CropAndResize(output_image.value(), + input_image.value(), src_rois[i], interp_type, stream)) { + return GXF_FAILURE; + } + } + + return GXF_SUCCESS; +} + +} // namespace detail + +gxf_result_t CropAndResize::registerInterface(gxf::Registrar* registrar) { + gxf::Expected result; + + result &= registrar->parameter( + output_width_, "output_width"); + result &= registrar->parameter( + output_height_, "output_height"); + result &= registrar->parameter( + interp_type_, "interp_type"); + result &= registrar->parameter( + keep_aspect_ratio_, "keep_aspect_ratio"); + result &= registrar->parameter( + receiver_bbox_, "receiver_bbox"); + result &= registrar->parameter( + receiver_, "receiver"); + result &= registrar->parameter( + transmitter_, "transmitter"); + result &= registrar->parameter( + pool_, "pool"); + result &= registrar->parameter( + stream_pool_, "stream_pool", + "cuda stream pool", "cuda stream pool object", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + result &= registrar->parameter( + input_adapter_, "input_adapter"); + result &= registrar->parameter( + output_adapter_, "output_adapter"); + result &= registrar->parameter( + input_name_, "input_name", "input name", "input tensor name", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + result &= registrar->parameter( + output_name_, "output_name", "output name", "output tensor name", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + + return gxf::ToResultCode(result); +} + +gxf::Expected CropAndResize::doInferOutputInfo(gxf::Entity& input) { + // Set crop regions + auto input_bbox_message = receiver_bbox_->receive(); + if (!input_bbox_message) { + return gxf::Unexpected{GXF_FAILURE}; + } + auto bbox_tensor = input_bbox_message.value().get(); + if (!bbox_tensor) { + return gxf::Unexpected{GXF_FAILURE}; + } + const gxf::Shape bbox_shape = bbox_tensor.value()->shape(); + if (bbox_shape.rank() != 2 || bbox_shape.dimension(1) != 4) { + GXF_LOG_ERROR("invalid input bbox dimension."); + return gxf::Unexpected{GXF_FAILURE}; + } + const size_t num_bbox = bbox_shape.dimension(0); + auto bbox_pointer = bbox_tensor.value()->data(); + if (!bbox_pointer) { + GXF_LOG_ERROR("empty bbox input."); + return gxf::Unexpected{GXF_FAILURE}; + } + std::vector rois; + for (size_t i = 0; i < num_bbox; i++) { + const int index = i * 4; + rois.push_back({bbox_pointer.value()[index], bbox_pointer.value()[index + 1], + bbox_pointer.value()[index + 2], + bbox_pointer.value()[index + 3]}); + } + input_rois_ = std::move(rois); + // Check if no-op is needed + no_op_ = input_rois_.size() == 1 && input_rois_[0].xmin == 0 && + input_rois_[0].xmax == static_cast(input_info_.width) && input_rois_[0].ymin == 0 && + input_rois_[0].ymax == static_cast(input_info_.height); + + return ImageInfo{input_info_.type, + output_width_.get(), output_height_.get(), input_info_.is_cpu}; +} + +gxf_result_t CropAndResize::doUpdateCameraMessage(gxf::Handle& output, + gxf::Handle& input) { + auto crop_result = GetCroppedCameraModel(*input, input_rois_[0]); + if (!crop_result) { + return GXF_FAILURE; + } + *output = GetScaledCameraModel(crop_result.value(), + output_info_.width, output_info_.height, false).value(); + return GXF_SUCCESS; +} + +#define DEFINE_CROP_AND_RESIZE(INPUT_TYPE) \ + if (input_info_.type == INPUT_TYPE) { \ + return detail::CropAndResizeImpl(output, input, output_info_, \ + input_info_, output_name, input_name, \ + output_adapter_.get(), input_adapter_.get(), pool_.get(), \ + input_rois_, interp.value(), stream); \ + } + +gxf_result_t CropAndResize::doExecute(gxf::Entity& output, + gxf::Entity& input, cudaStream_t stream, + const char* output_name, const char* input_name) { + // Check if interpolation type is valid + auto interp = GetInterpolationType(interp_type_); + if (!interp) { + return interp.error(); + } + + // Run the image resizing operation + DEFINE_CROP_AND_RESIZE(cvcore::tensor_ops::ImageType::Y_U8); + DEFINE_CROP_AND_RESIZE(cvcore::tensor_ops::ImageType::Y_U16); + DEFINE_CROP_AND_RESIZE(cvcore::tensor_ops::ImageType::Y_F32); + DEFINE_CROP_AND_RESIZE(cvcore::tensor_ops::ImageType::RGB_U8); + DEFINE_CROP_AND_RESIZE(cvcore::tensor_ops::ImageType::RGB_U16); + DEFINE_CROP_AND_RESIZE(cvcore::tensor_ops::ImageType::RGB_F32); + DEFINE_CROP_AND_RESIZE(cvcore::tensor_ops::ImageType::BGR_U8); + DEFINE_CROP_AND_RESIZE(cvcore::tensor_ops::ImageType::BGR_U16); + DEFINE_CROP_AND_RESIZE(cvcore::tensor_ops::ImageType::BGR_F32); + + // Return error code for unsupported type + GXF_LOG_ERROR("invalid input/output type for image crop_and_resize."); + return GXF_FAILURE; +} + +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/CropAndResize.hpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/CropAndResize.hpp similarity index 72% rename from isaac_ros_image_proc/gxf/tensorops/CropAndResize.hpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/CropAndResize.hpp index 2bde6fb..aa88f1e 100644 --- a/isaac_ros_image_proc/gxf/tensorops/CropAndResize.hpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/CropAndResize.hpp @@ -14,40 +14,41 @@ // limitations under the License. // // SPDX-License-Identifier: Apache-2.0 -#ifndef NVIDIA_CVCORE_CROP_AND_RESIZE_HPP -#define NVIDIA_CVCORE_CROP_AND_RESIZE_HPP +#pragma once -#include "TensorOperator.hpp" -#include "cv/core/BBox.h" +#include +#include + +#include "extensions/tensorops/components/TensorOperator.hpp" +#include "extensions/tensorops/core/BBox.h" namespace nvidia { -namespace cvcore { +namespace isaac { namespace tensor_ops { // CropAndResize operator. class CropAndResize : public TensorOperator { -public: + public: virtual ~CropAndResize() {} gxf_result_t registerInterface(gxf::Registrar* registrar) override; -private: - gxf::Expected doInferOutputInfo(gxf::Entity& input) override final; + private: + gxf::Expected doInferOutputInfo(gxf::Entity& input) override; gxf_result_t doUpdateCameraMessage(gxf::Handle& output, - gxf::Handle& input) override final; - gxf_result_t doExecute(gxf::Entity& output, gxf::Entity& input, cudaStream_t stream, const char* output_name, - const char* input_name) override final; + gxf::Handle& input) override; + gxf_result_t doExecute(gxf::Entity& output, gxf::Entity& input, + cudaStream_t stream, const char* output_name, + const char* input_name) override; gxf::Parameter output_width_; gxf::Parameter output_height_; gxf::Parameter interp_type_; gxf::Parameter keep_aspect_ratio_; gxf::Parameter> receiver_bbox_; - std::vector<::cvcore::BBox> input_rois_; + std::vector input_rois_; }; -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia - -#endif +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/Frame3D.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Frame3D.cpp similarity index 91% rename from isaac_ros_image_proc/gxf/tensorops/Frame3D.cpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Frame3D.cpp index c28829f..58468c6 100644 --- a/isaac_ros_image_proc/gxf/tensorops/Frame3D.cpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Frame3D.cpp @@ -18,7 +18,7 @@ #include "Frame3D.hpp" namespace nvidia { -namespace cvcore { +namespace isaac { namespace tensor_ops { gxf_result_t Frame3D::registerInterface(gxf::Registrar* registrar) { @@ -47,10 +47,10 @@ gxf_result_t Frame3D::initialize() { for (size_t i = 0; i < 3; i++) { raw_matrix[i][3] = translation_.get()[i]; } - extrinsics_ = ::cvcore::CameraExtrinsics(raw_matrix); + extrinsics_ = cvcore::tensor_ops::CameraExtrinsics(raw_matrix); return GXF_SUCCESS; } -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/Frame3D.hpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Frame3D.hpp similarity index 79% rename from isaac_ros_image_proc/gxf/tensorops/Frame3D.hpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Frame3D.hpp index aec8cf5..14f4a47 100644 --- a/isaac_ros_image_proc/gxf/tensorops/Frame3D.hpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Frame3D.hpp @@ -14,40 +14,38 @@ // limitations under the License. // // SPDX-License-Identifier: Apache-2.0 -#ifndef NVIDIA_CVCORE_FRAME3D_HPP -#define NVIDIA_CVCORE_FRAME3D_HPP +#pragma once +#include #include "gxf/core/component.hpp" #include "gxf/std/parameter_parser_std.hpp" -#include "cv/core/CameraModel.h" +#include "extensions/tensorops/core/CameraModel.h" namespace nvidia { -namespace cvcore { +namespace isaac { namespace tensor_ops { // Wrapper of CameraExtrinsics compatible with CVCORE class Frame3D : public gxf::Component { -public: + public: virtual ~Frame3D() = default; Frame3D() = default; gxf_result_t registerInterface(gxf::Registrar* registrar) override; gxf_result_t initialize() override; - ::cvcore::CameraExtrinsics getCameraExtrinsics() const { + cvcore::tensor_ops::CameraExtrinsics getCameraExtrinsics() const { return extrinsics_; } -private: + private: gxf::Parameter> rotation_; gxf::Parameter> translation_; - ::cvcore::CameraExtrinsics extrinsics_; + cvcore::tensor_ops::CameraExtrinsics extrinsics_; }; -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia - -#endif +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/ImageAdapter.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/ImageAdapter.cpp similarity index 80% rename from isaac_ros_image_proc/gxf/tensorops/ImageAdapter.cpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/ImageAdapter.cpp index 12e7fe7..0040bbb 100644 --- a/isaac_ros_image_proc/gxf/tensorops/ImageAdapter.cpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/ImageAdapter.cpp @@ -17,17 +17,20 @@ #include "ImageAdapter.hpp" namespace nvidia { -namespace cvcore { +namespace isaac { namespace tensor_ops { gxf_result_t ImageAdapter::registerInterface(gxf::Registrar* registrar) { gxf::Expected result; - result &= registrar->parameter(message_type_param_, "message_type"); - result &= registrar->parameter(image_type_param_, "image_type", "image type", "optional image type", - gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); - result &= registrar->parameter(allocate_pitch_linear_, "allocate_pitch_linear", - "if true, allocate output buffers as padded pitch linear surfaces", "", false); + result &= registrar->parameter( + message_type_param_, "message_type"); + result &= registrar->parameter( + image_type_param_, "image_type", "image type", "optional image type", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + result &= registrar->parameter( + allocate_pitch_linear_, "allocate_pitch_linear", + "if true, allocate output buffers as padded pitch linear surfaces", "", false); return gxf::ToResultCode(result); } @@ -73,6 +76,6 @@ gxf::Expected ImageAdapter::GetImageInfo(const gxf::Entity& message, } } -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/ImageAdapter.hpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/ImageAdapter.hpp similarity index 74% rename from isaac_ros_image_proc/gxf/tensorops/ImageAdapter.hpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/ImageAdapter.hpp index 3b41391..5c2b608 100644 --- a/isaac_ros_image_proc/gxf/tensorops/ImageAdapter.hpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/ImageAdapter.hpp @@ -14,23 +14,22 @@ // limitations under the License. // // SPDX-License-Identifier: Apache-2.0 -#ifndef NVIDIA_CVCORE_IMAGE_ADAPTER_HPP -#define NVIDIA_CVCORE_IMAGE_ADAPTER_HPP +#pragma once -#include "ImageUtils.hpp" -#include "detail/ImageAdapterTensorImpl.hpp" -#include "detail/ImageAdapterVideoBufferImpl.hpp" +#include +#include "extensions/tensorops/components/detail/ImageAdapterTensorImpl.hpp" +#include "extensions/tensorops/components/detail/ImageAdapterVideoBufferImpl.hpp" +#include "extensions/tensorops/components/ImageUtils.hpp" +#include "extensions/tensorops/core/Image.h" #include "gxf/core/component.hpp" #include "gxf/multimedia/video.hpp" #include "gxf/std/allocator.hpp" #include "gxf/std/parameter_parser_std.hpp" #include "gxf/std/tensor.hpp" -#include "cv/core/Image.h" - namespace nvidia { -namespace cvcore { +namespace isaac { namespace tensor_ops { // Enum class: gxf::Tensor and gxf::VideoBuffer @@ -41,7 +40,7 @@ enum class BufferType { // Utility component for conversion between message and cvcore image type class ImageAdapter : public gxf::Component { -public: + public: virtual ~ImageAdapter() = default; ImageAdapter() = default; @@ -50,8 +49,9 @@ class ImageAdapter : public gxf::Component { gxf::Expected GetImageInfo(const gxf::Entity& message, const char* name = nullptr); - template<::cvcore::ImageType T> - gxf::Expected<::cvcore::Image> WrapImageFromMessage(const gxf::Entity& message, const char* name = nullptr) { + template + gxf::Expected> WrapImageFromMessage(const gxf::Entity& message, + const char* name = nullptr) { if (message_type_ == BufferType::TENSOR) { auto tensor = message.get(name); if (!tensor) { @@ -67,35 +67,36 @@ class ImageAdapter : public gxf::Component { } } - template<::cvcore::ImageType T> + template gxf_result_t AddImageToMessage(gxf::Entity& message, size_t width, size_t height, - gxf::Handle allocator, bool is_cpu, const char* name = nullptr) { + gxf::Handle allocator, + bool is_cpu, const char* name = nullptr) { if (message_type_ == BufferType::TENSOR) { auto tensor = message.add(name); if (!tensor) { return GXF_FAILURE; } - return detail::AllocateTensor(tensor.value(), width, height, allocator, is_cpu, allocate_pitch_linear_.get()); + return detail::AllocateTensor(tensor.value(), width, height, + allocator, is_cpu, allocate_pitch_linear_.get()); } else { auto video_buffer = message.add(name); if (!video_buffer) { return GXF_FAILURE; } - return detail::AllocateVideoBuffer(video_buffer.value(), width, height, allocator, is_cpu, allocate_pitch_linear_.get()); + return detail::AllocateVideoBuffer(video_buffer.value(), width, + height, allocator, is_cpu, allocate_pitch_linear_.get()); } } -private: + private: gxf::Parameter message_type_param_; gxf::Parameter image_type_param_; gxf::Parameter allocate_pitch_linear_; - ::cvcore::ImageType image_type_; + cvcore::tensor_ops::ImageType image_type_; BufferType message_type_; }; -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia - -#endif +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/ImageUtils.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/ImageUtils.cpp similarity index 56% rename from isaac_ros_image_proc/gxf/tensorops/ImageUtils.cpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/ImageUtils.cpp index 6c0346f..afa9817 100644 --- a/isaac_ros_image_proc/gxf/tensorops/ImageUtils.cpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/ImageUtils.cpp @@ -18,115 +18,119 @@ #include #include +#include namespace nvidia { -namespace cvcore { +namespace isaac { namespace tensor_ops { // helper function to match input image type string to cvcore::ImageType -gxf::Expected<::cvcore::ImageType> GetImageTypeFromString(const std::string& type) { +gxf::Expected GetImageTypeFromString(const std::string& type) { if (type == "Y_U8") { - return ::cvcore::ImageType::Y_U8; + return cvcore::tensor_ops::ImageType::Y_U8; } else if (type == "Y_U16") { - return ::cvcore::ImageType::Y_U16; + return cvcore::tensor_ops::ImageType::Y_U16; } else if (type == "Y_F32") { - return ::cvcore::ImageType::Y_F32; + return cvcore::tensor_ops::ImageType::Y_F32; } else if (type == "RGB_U8") { - return ::cvcore::ImageType::RGB_U8; + return cvcore::tensor_ops::ImageType::RGB_U8; } else if (type == "RGB_U16") { - return ::cvcore::ImageType::RGB_U16; + return cvcore::tensor_ops::ImageType::RGB_U16; } else if (type == "RGB_F32") { - return ::cvcore::ImageType::RGB_F32; + return cvcore::tensor_ops::ImageType::RGB_F32; } else if (type == "BGR_U8") { - return ::cvcore::ImageType::BGR_U8; + return cvcore::tensor_ops::ImageType::BGR_U8; } else if (type == "BGR_U16") { - return ::cvcore::ImageType::BGR_U16; + return cvcore::tensor_ops::ImageType::BGR_U16; } else if (type == "BGR_F32") { - return ::cvcore::ImageType::BGR_F32; + return cvcore::tensor_ops::ImageType::BGR_F32; } else if (type == "PLANAR_RGB_U8") { - return ::cvcore::ImageType::PLANAR_RGB_U8; + return cvcore::tensor_ops::ImageType::PLANAR_RGB_U8; } else if (type == "PLANAR_RGB_U16") { - return ::cvcore::ImageType::PLANAR_RGB_U16; + return cvcore::tensor_ops::ImageType::PLANAR_RGB_U16; } else if (type == "PLANAR_RGB_F32") { - return ::cvcore::ImageType::PLANAR_RGB_F32; + return cvcore::tensor_ops::ImageType::PLANAR_RGB_F32; } else if (type == "PLANAR_BGR_U8") { - return ::cvcore::ImageType::PLANAR_BGR_U8; + return cvcore::tensor_ops::ImageType::PLANAR_BGR_U8; } else if (type == "PLANAR_BGR_U16") { - return ::cvcore::ImageType::PLANAR_BGR_U16; + return cvcore::tensor_ops::ImageType::PLANAR_BGR_U16; } else if (type == "PLANAR_BGR_F32") { - return ::cvcore::ImageType::PLANAR_BGR_F32; + return cvcore::tensor_ops::ImageType::PLANAR_BGR_F32; } else if (type == "NV12") { - return ::cvcore::ImageType::NV12; + return cvcore::tensor_ops::ImageType::NV12; } else if (type == "NV24") { - return ::cvcore::ImageType::NV24; + return cvcore::tensor_ops::ImageType::NV24; } else { GXF_LOG_ERROR("invalid image type."); return gxf::Unexpected{GXF_FAILURE}; } } -gxf::Expected<::cvcore::tensor_ops::InterpolationType> GetInterpolationType(const std::string& type) { +gxf::Expected GetInterpolationType( + const std::string& type) { if (type == "nearest") { - return ::cvcore::tensor_ops::InterpolationType::INTERP_NEAREST; + return cvcore::tensor_ops::InterpolationType::INTERP_NEAREST; } else if (type == "linear") { - return ::cvcore::tensor_ops::InterpolationType::INTERP_LINEAR; + return cvcore::tensor_ops::InterpolationType::INTERP_LINEAR; } else if (type == "cubic_bspline") { - return ::cvcore::tensor_ops::InterpolationType::INTERP_CUBIC_BSPLINE; + return cvcore::tensor_ops::InterpolationType::INTERP_CUBIC_BSPLINE; } else if (type == "cubic_catmullrom") { - return ::cvcore::tensor_ops::InterpolationType::INTERP_CUBIC_CATMULLROM; + return cvcore::tensor_ops::InterpolationType::INTERP_CUBIC_CATMULLROM; } else { GXF_LOG_ERROR("invalid interpolation type."); return gxf::Unexpected{GXF_FAILURE}; } } -gxf::Expected<::cvcore::tensor_ops::BorderType> GetBorderType(const std::string& type) { +gxf::Expected GetBorderType(const std::string& type) { if (type == "zero") { - return ::cvcore::tensor_ops::BorderType::BORDER_ZERO; + return cvcore::tensor_ops::BorderType::BORDER_ZERO; } else if (type == "repeat") { - return ::cvcore::tensor_ops::BorderType::BORDER_REPEAT; + return cvcore::tensor_ops::BorderType::BORDER_REPEAT; } else if (type == "reverse") { - return ::cvcore::tensor_ops::BorderType::BORDER_REVERSE; + return cvcore::tensor_ops::BorderType::BORDER_REVERSE; } else if (type == "mirror") { - return ::cvcore::tensor_ops::BorderType::BORDER_MIRROR; + return cvcore::tensor_ops::BorderType::BORDER_MIRROR; } else { GXF_LOG_ERROR("invalid border type."); return gxf::Unexpected{GXF_FAILURE}; } } -gxf::Expected<::cvcore::CameraDistortionType> GetCameraDistortionType(gxf::DistortionType type) { +gxf::Expected GetCameraDistortionType( + gxf::DistortionType type) { switch (type) { case gxf::DistortionType::Perspective: - return ::cvcore::CameraDistortionType::NONE; + return cvcore::tensor_ops::CameraDistortionType::NONE; case gxf::DistortionType::Brown: case gxf::DistortionType::Polynomial: - return ::cvcore::CameraDistortionType::Polynomial; + return cvcore::tensor_ops::CameraDistortionType::Polynomial; case gxf::DistortionType::FisheyeEquidistant: - return ::cvcore::CameraDistortionType::FisheyeEquidistant; + return cvcore::tensor_ops::CameraDistortionType::FisheyeEquidistant; case gxf::DistortionType::FisheyeEquisolid: - return ::cvcore::CameraDistortionType::FisheyeEquisolid; + return cvcore::tensor_ops::CameraDistortionType::FisheyeEquisolid; case gxf::DistortionType::FisheyeOrthoGraphic: - return ::cvcore::CameraDistortionType::FisheyeOrthoGraphic; + return cvcore::tensor_ops::CameraDistortionType::FisheyeOrthoGraphic; case gxf::DistortionType::FisheyeStereographic: - return ::cvcore::CameraDistortionType::FisheyeStereographic; + return cvcore::tensor_ops::CameraDistortionType::FisheyeStereographic; default: GXF_LOG_ERROR("invalid distortion type."); return gxf::Unexpected{GXF_FAILURE}; } } -gxf::Expected GetDistortionType(::cvcore::CameraDistortionType type) { +gxf::Expected GetDistortionType( + cvcore::tensor_ops::CameraDistortionType type) { switch (type) { - case ::cvcore::CameraDistortionType::Polynomial: + case cvcore::tensor_ops::CameraDistortionType::Polynomial: return gxf::DistortionType::Polynomial; - case ::cvcore::CameraDistortionType::FisheyeEquidistant: + case cvcore::tensor_ops::CameraDistortionType::FisheyeEquidistant: return gxf::DistortionType::FisheyeEquidistant; - case ::cvcore::CameraDistortionType::FisheyeEquisolid: + case cvcore::tensor_ops::CameraDistortionType::FisheyeEquisolid: return gxf::DistortionType::FisheyeEquisolid; - case ::cvcore::CameraDistortionType::FisheyeOrthoGraphic: + case cvcore::tensor_ops::CameraDistortionType::FisheyeOrthoGraphic: return gxf::DistortionType::FisheyeOrthoGraphic; - case ::cvcore::CameraDistortionType::FisheyeStereographic: + case cvcore::tensor_ops::CameraDistortionType::FisheyeStereographic: return gxf::DistortionType::FisheyeStereographic; default: GXF_LOG_ERROR("invalid distortion type."); @@ -134,14 +138,17 @@ gxf::Expected GetDistortionType(::cvcore::CameraDistortionT } } -gxf::Expected GetCroppedCameraModel(const gxf::CameraModel& input, const ::cvcore::BBox& roi) { +gxf::Expected GetCroppedCameraModel( + const gxf::CameraModel& input, + const cvcore::tensor_ops::BBox& roi) { if (!roi.isValid()) { return gxf::Unexpected{GXF_FAILURE}; } gxf::CameraModel camera; const size_t output_width = roi.xmax - roi.xmin; const size_t output_height = roi.ymax - roi.ymin; - camera.dimensions = {static_cast(output_width), static_cast(output_height)}; + camera.dimensions = {static_cast(output_width), + static_cast(output_height)}; camera.focal_length = input.focal_length; // We will keep the relative principal point location unchanged for cropping; camera.principal_point = {input.principal_point.x / input.dimensions.x * output_width, @@ -153,16 +160,18 @@ gxf::Expected GetCroppedCameraModel(const gxf::CameraModel& in return camera; } -gxf::Expected GetScaledCameraModel(const gxf::CameraModel& input, size_t output_width, - size_t output_height, bool keep_aspect_ratio) { +gxf::Expected GetScaledCameraModel( + const gxf::CameraModel& input, size_t output_width, + size_t output_height, bool keep_aspect_ratio) { gxf::CameraModel camera; const float scaler_x = static_cast(output_width) / input.dimensions.x; const float scaler_y = static_cast(output_height) / input.dimensions.y; const float min_scaler = std::min(scaler_x, scaler_y); - camera.dimensions = {static_cast(output_width), static_cast(output_height)}; + camera.dimensions = {static_cast(output_width), + static_cast(output_height)}; camera.focal_length = keep_aspect_ratio - ? nvidia::gxf::Vector2f{min_scaler * input.focal_length.x, min_scaler * input.focal_length.y} - : nvidia::gxf::Vector2f{scaler_x * input.focal_length.x, scaler_y * input.focal_length.y}; + ? nvidia::gxf::Vector2f{min_scaler * input.focal_length.x, min_scaler * input.focal_length.y} + : nvidia::gxf::Vector2f{scaler_x * input.focal_length.x, scaler_y * input.focal_length.y}; camera.principal_point = {scaler_x * input.principal_point.x, scaler_y * input.principal_point.y}, camera.skew_value = input.skew_value; camera.distortion_type = input.distortion_type; @@ -171,6 +180,6 @@ gxf::Expected GetScaledCameraModel(const gxf::CameraModel& inp return camera; } -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/ImageUtils.hpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/ImageUtils.hpp similarity index 54% rename from isaac_ros_image_proc/gxf/tensorops/ImageUtils.hpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/ImageUtils.hpp index d052827..0b7bf54 100644 --- a/isaac_ros_image_proc/gxf/tensorops/ImageUtils.hpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/ImageUtils.hpp @@ -14,52 +14,54 @@ // limitations under the License. // // SPDX-License-Identifier: Apache-2.0 -#ifndef NVIDIA_CVCORE_IMAGE_UTILS_HPP -#define NVIDIA_CVCORE_IMAGE_UTILS_HPP +#pragma once -#include "cv/core/BBox.h" -#include "cv/core/CameraModel.h" -#include "cv/core/Image.h" -#include "cv/tensor_ops/ImageUtils.h" +#include +#include "extensions/tensorops/core/BBox.h" +#include "extensions/tensorops/core/CameraModel.h" +#include "extensions/tensorops/core/Image.h" +#include "extensions/tensorops/core/ImageUtils.h" #include "gxf/core/expected.hpp" #include "gxf/multimedia/camera.hpp" namespace nvidia { -namespace cvcore { +namespace isaac { namespace tensor_ops { // Description of Image struct ImageInfo { - ::cvcore::ImageType type; + cvcore::tensor_ops::ImageType type; size_t width; size_t height; bool is_cpu; }; // helper function to match input image type string to cvcore::ImageType -gxf::Expected<::cvcore::ImageType> GetImageTypeFromString(const std::string& type); +gxf::Expected GetImageTypeFromString(const std::string& type); // Helper function to get the interpolation type -gxf::Expected<::cvcore::tensor_ops::InterpolationType> GetInterpolationType(const std::string& type); +gxf::Expected GetInterpolationType( + const std::string& type); // Helper function to get the border type -gxf::Expected<::cvcore::tensor_ops::BorderType> GetBorderType(const std::string& type); +gxf::Expected GetBorderType(const std::string& type); // Helper function to get the cvcore camera distortion type -gxf::Expected<::cvcore::CameraDistortionType> GetCameraDistortionType(gxf::DistortionType type); +gxf::Expected GetCameraDistortionType( + gxf::DistortionType type); // Helper function to get the gxf distortion type -gxf::Expected GetDistortionType(::cvcore::CameraDistortionType type); +gxf::Expected GetDistortionType(cvcore::tensor_ops::CameraDistortionType type); // Helper function to get the new camera model after applying crop operation -gxf::Expected GetCroppedCameraModel(const gxf::CameraModel& input, const ::cvcore::BBox& roi); +gxf::Expected GetCroppedCameraModel( + const gxf::CameraModel& input, const cvcore::tensor_ops::BBox& roi); // Helper function to get the new camera model after applying scale operation -gxf::Expected GetScaledCameraModel(const gxf::CameraModel& input, size_t output_width, - size_t output_height, bool keep_aspect_ratio); +gxf::Expected GetScaledCameraModel( + const gxf::CameraModel& input, size_t output_width, + size_t output_height, bool keep_aspect_ratio); -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia - -#endif +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/InterleavedToPlanar.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/InterleavedToPlanar.cpp new file mode 100644 index 0000000..f920033 --- /dev/null +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/InterleavedToPlanar.cpp @@ -0,0 +1,165 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 +#include "InterleavedToPlanar.hpp" + +namespace nvidia { +namespace isaac { +namespace tensor_ops { + +namespace detail { + +template +gxf_result_t InterleavedToPlanarImpl( + gxf::Entity& output, gxf::Entity& input, const ImageInfo& output_info, + const ImageInfo& input_info, const char* output_name, const char* input_name, + gxf::Handle output_adapter, gxf::Handle input_adapter, + gxf::Handle allocator, cudaStream_t stream) { + auto input_image = input_adapter->WrapImageFromMessage(input, input_name); + if (!input_image) { + return GXF_FAILURE; + } + + auto error = output_adapter->AddImageToMessage( + output, output_info.width, output_info.height, allocator, + output_info.is_cpu, output_name); + if (error != GXF_SUCCESS) { + return GXF_FAILURE; + } + + auto output_image = output_adapter->WrapImageFromMessage(output, output_name); + if (!output_image) { + return GXF_FAILURE; + } + cvcore::tensor_ops::InterleavedToPlanar(output_image.value(), input_image.value(), stream); + return GXF_SUCCESS; +} + +} // namespace detail + +gxf_result_t InterleavedToPlanar::registerInterface(gxf::Registrar* registrar) { + gxf::Expected result; + + result &= registrar->parameter( + receiver_, "receiver"); + result &= registrar->parameter( + transmitter_, "transmitter"); + result &= registrar->parameter( + pool_, "pool"); + result &= registrar->parameter( + stream_pool_, "stream_pool", "cuda stream pool", "cuda stream pool object", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + result &= registrar->parameter( + input_adapter_, "input_adapter"); + result &= registrar->parameter( + output_adapter_, "output_adapter"); + result &= registrar->parameter( + input_name_, "input_name", + "input name", "input tensor name", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + result &= registrar->parameter( + output_name_, "output_name", + "output name", "output tensor name", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + + return gxf::ToResultCode(result); +} + +gxf::Expected InterleavedToPlanar::doInferOutputInfo(gxf::Entity& input) { + // Output type is planar + cvcore::tensor_ops::ImageType output_type; + switch (input_info_.type) { + case cvcore::tensor_ops::ImageType::RGB_U8: { + output_type = cvcore::tensor_ops::ImageType::PLANAR_RGB_U8; + break; + } + case cvcore::tensor_ops::ImageType::RGB_U16: { + output_type = cvcore::tensor_ops::ImageType::PLANAR_RGB_U16; + break; + } + case cvcore::tensor_ops::ImageType::RGB_F32: { + output_type = cvcore::tensor_ops::ImageType::PLANAR_RGB_F32; + break; + } + case cvcore::tensor_ops::ImageType::BGR_U8: { + output_type = cvcore::tensor_ops::ImageType::PLANAR_BGR_U8; + break; + } + case cvcore::tensor_ops::ImageType::BGR_U16: { + output_type = cvcore::tensor_ops::ImageType::PLANAR_BGR_U16; + break; + } + case cvcore::tensor_ops::ImageType::BGR_F32: { + output_type = cvcore::tensor_ops::ImageType::PLANAR_BGR_F32; + break; + } + case cvcore::tensor_ops::ImageType::PLANAR_RGB_U8: + case cvcore::tensor_ops::ImageType::PLANAR_RGB_U16: + case cvcore::tensor_ops::ImageType::PLANAR_RGB_F32: + case cvcore::tensor_ops::ImageType::PLANAR_BGR_U8: + case cvcore::tensor_ops::ImageType::PLANAR_BGR_U16: + case cvcore::tensor_ops::ImageType::PLANAR_BGR_F32: { + output_type = input_info_.type; + no_op_ = true; + break; + } + default: { + GXF_LOG_ERROR("invalid input type for interleaved to planar conversion."); + return gxf::Unexpected{GXF_FAILURE}; + } + } + return ImageInfo{output_type, input_info_.width, input_info_.height, input_info_.is_cpu}; +} + +gxf_result_t InterleavedToPlanar::doUpdateCameraMessage(gxf::Handle& output, + gxf::Handle& input) { + *output = *input; + return GXF_SUCCESS; +} + +#define DEFINE_INTERLEAVED_TO_PLANAR(INPUT_TYPE, OUTPUT_TYPE) \ + if (input_info_.type == INPUT_TYPE && output_info_.type == OUTPUT_TYPE) { \ + return detail::InterleavedToPlanarImpl( \ + output, input, output_info_, input_info_, \ + output_name, input_name, output_adapter_.get(), \ + input_adapter_.get(), pool_.get(), stream); \ + } + +gxf_result_t InterleavedToPlanar::doExecute( + gxf::Entity& output, gxf::Entity& input, cudaStream_t stream, + const char* output_name, const char* input_name) { + // Run the interleaved to planar operation + DEFINE_INTERLEAVED_TO_PLANAR(cvcore::tensor_ops::ImageType::RGB_U8, + cvcore::tensor_ops::ImageType::PLANAR_RGB_U8); + DEFINE_INTERLEAVED_TO_PLANAR(cvcore::tensor_ops::ImageType::RGB_U16, + cvcore::tensor_ops::ImageType::PLANAR_RGB_U16); + DEFINE_INTERLEAVED_TO_PLANAR(cvcore::tensor_ops::ImageType::RGB_F32, + cvcore::tensor_ops::ImageType::PLANAR_RGB_F32); + DEFINE_INTERLEAVED_TO_PLANAR(cvcore::tensor_ops::ImageType::BGR_U8, + cvcore::tensor_ops::ImageType::PLANAR_BGR_U8); + DEFINE_INTERLEAVED_TO_PLANAR(cvcore::tensor_ops::ImageType::BGR_U16, + cvcore::tensor_ops::ImageType::PLANAR_BGR_U16); + DEFINE_INTERLEAVED_TO_PLANAR(cvcore::tensor_ops::ImageType::BGR_F32, + cvcore::tensor_ops::ImageType::PLANAR_BGR_F32); + + // Return error code for unsupported type + GXF_LOG_ERROR("invalid input/output type for image interleaved to planar conversion."); + return GXF_FAILURE; +} + +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/InterleavedToPlanar.hpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/InterleavedToPlanar.hpp similarity index 75% rename from isaac_ros_image_proc/gxf/tensorops/InterleavedToPlanar.hpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/InterleavedToPlanar.hpp index 2e1efbe..8062084 100644 --- a/isaac_ros_image_proc/gxf/tensorops/InterleavedToPlanar.hpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/InterleavedToPlanar.hpp @@ -14,32 +14,30 @@ // limitations under the License. // // SPDX-License-Identifier: Apache-2.0 -#ifndef NVIDIA_CVCORE_INTERLEAVED_TO_PLANAR_HPP -#define NVIDIA_CVCORE_INTERLEAVED_TO_PLANAR_HPP +#pragma once -#include "TensorOperator.hpp" +#include "extensions/tensorops/components/TensorOperator.hpp" namespace nvidia { -namespace cvcore { +namespace isaac { namespace tensor_ops { // InterleavedToPlanar operator. class InterleavedToPlanar : public TensorOperator { -public: + public: virtual ~InterleavedToPlanar() {} gxf_result_t registerInterface(gxf::Registrar* registrar) override; -private: - gxf::Expected doInferOutputInfo(gxf::Entity& input) override final; + private: + gxf::Expected doInferOutputInfo(gxf::Entity& input) override; gxf_result_t doUpdateCameraMessage(gxf::Handle& output, - gxf::Handle& input) override final; - gxf_result_t doExecute(gxf::Entity& output, gxf::Entity& input, cudaStream_t stream, const char* output_name, - const char* input_name) override final; + gxf::Handle& input) override; + gxf_result_t doExecute(gxf::Entity& output, gxf::Entity& input, + cudaStream_t stream, const char* output_name, + const char* input_name) override; }; -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia - -#endif +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Normalize.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Normalize.cpp new file mode 100644 index 0000000..fc08f10 --- /dev/null +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Normalize.cpp @@ -0,0 +1,216 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 +#include "Normalize.hpp" + +#include +namespace nvidia { +namespace isaac { +namespace tensor_ops { + +namespace detail { + +template +gxf_result_t NormalizeC1Impl(gxf::Entity& output, gxf::Entity& input, const ImageInfo& output_info, + const ImageInfo& input_info, const char* output_name, const char* input_name, + gxf::Handle output_adapter, gxf::Handle input_adapter, + gxf::Handle allocator, const std::vector& scales, + const std::vector& offsets, cudaStream_t stream) { + if (scales.size() != 1 || offsets.size() != 1) { + GXF_LOG_ERROR("invalid scales/offsets dimension"); + return GXF_FAILURE; + } + + auto input_image = input_adapter->WrapImageFromMessage(input, input_name); + if (!input_image) { + return GXF_FAILURE; + } + + auto error = output_adapter->AddImageToMessage(output, + output_info.width, output_info.height, + allocator, output_info.is_cpu, output_name); + if (error != GXF_SUCCESS) { + return GXF_FAILURE; + } + + auto output_image = output_adapter->WrapImageFromMessage(output, output_name); + if (!output_image) { + return GXF_FAILURE; + } + if (cvcore::tensor_ops::Normalize(output_image.value(), input_image.value(), + scales[0], offsets[0], stream)) { + return GXF_SUCCESS; + } else { + return GXF_FAILURE; + } +} + +template +gxf_result_t NormalizeC3Impl(gxf::Entity& output, gxf::Entity& input, const ImageInfo& output_info, + const ImageInfo& input_info, const char* output_name, const char* input_name, + gxf::Handle output_adapter, gxf::Handle input_adapter, + gxf::Handle allocator, const std::vector& scales, + const std::vector& offsets, cudaStream_t stream) { + if (scales.size() != 3 || offsets.size() != 3) { + GXF_LOG_ERROR("invalid scales/offsets dimension"); + return GXF_FAILURE; + } + + auto input_image = input_adapter->WrapImageFromMessage(input, input_name); + if (!input_image) { + return GXF_FAILURE; + } + + auto error = output_adapter->AddImageToMessage(output, output_info.width, + output_info.height, allocator, + output_info.is_cpu, output_name); + if (error != GXF_SUCCESS) { + return GXF_FAILURE; + } + + auto output_image = output_adapter->WrapImageFromMessage(output, output_name); + if (!output_image) { + return GXF_FAILURE; + } + const float scales_value[3] = {scales[0], scales[1], scales[2]}; + const float offsets_value[3] = {offsets[0], offsets[1], offsets[2]}; + if (cvcore::tensor_ops::Normalize(output_image.value(), input_image.value(), + scales_value, offsets_value, stream)) { + return GXF_SUCCESS; + } else { + return GXF_FAILURE; + } +} + +} // namespace detail + +gxf_result_t Normalize::registerInterface(gxf::Registrar* registrar) { + gxf::Expected result; + + result &= registrar->parameter( + scales_, "scales"); + result &= registrar->parameter( + offsets_, "offsets"); + result &= registrar->parameter( + receiver_, "receiver"); + result &= registrar->parameter( + transmitter_, "transmitter"); + result &= registrar->parameter( + pool_, "pool"); + result &= registrar->parameter( + stream_pool_, "stream_pool", + "cuda stream pool", "cuda stream pool object", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + result &= registrar->parameter( + input_adapter_, "input_adapter"); + result &= registrar->parameter( + output_adapter_, "output_adapter"); + result &= registrar->parameter( + input_name_, "input_name", "input name", "input tensor name", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + result &= registrar->parameter( + output_name_, "output_name", "output name", "output tensor name", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + + return gxf::ToResultCode(result); +} + +gxf::Expected Normalize::doInferOutputInfo(gxf::Entity& input) { + // Output type is F32 + cvcore::tensor_ops::ImageType output_type; + switch (input_info_.type) { + case cvcore::tensor_ops::ImageType::Y_U8: + case cvcore::tensor_ops::ImageType::Y_U16: + case cvcore::tensor_ops::ImageType::Y_F32: { + output_type = cvcore::tensor_ops::ImageType::Y_F32; + break; + } + case cvcore::tensor_ops::ImageType::RGB_U8: + case cvcore::tensor_ops::ImageType::RGB_U16: + case cvcore::tensor_ops::ImageType::RGB_F32: { + output_type = cvcore::tensor_ops::ImageType::RGB_F32; + break; + } + case cvcore::tensor_ops::ImageType::BGR_U8: + case cvcore::tensor_ops::ImageType::BGR_U16: + case cvcore::tensor_ops::ImageType::BGR_F32: { + output_type = cvcore::tensor_ops::ImageType::BGR_F32; + break; + } + default: { + GXF_LOG_ERROR("invalid input type for normalize."); + return gxf::Unexpected{GXF_FAILURE}; + } + } + // Operation must be performed under any condition + no_op_ = false; + return ImageInfo{output_type, input_info_.width, input_info_.height, input_info_.is_cpu}; +} + +gxf_result_t Normalize::doUpdateCameraMessage(gxf::Handle& output, + gxf::Handle& input) { + *output = *input; + return GXF_SUCCESS; +} + +#define DEFINE_NORMALIZE_C1(INPUT_TYPE, OUTPUT_TYPE) \ + if (input_info_.type == INPUT_TYPE && output_info_.type == OUTPUT_TYPE) { \ + return detail::NormalizeC1Impl( \ + output, input, output_info_, input_info_, output_name, \ + input_name, output_adapter_.get(), input_adapter_.get(), \ + pool_.get(), scales_.get(), offsets_.get(), stream); \ + } + +#define DEFINE_NORMALIZE_C3(INPUT_TYPE, OUTPUT_TYPE) \ + if (input_info_.type == INPUT_TYPE && output_info_.type == OUTPUT_TYPE) { \ + return detail::NormalizeC3Impl( \ + output, input, output_info_, input_info_, output_name, \ + input_name, output_adapter_.get(), input_adapter_.get(), \ + pool_.get(), scales_.get(), offsets_.get(), stream); \ + } + +gxf_result_t Normalize::doExecute(gxf::Entity& output, gxf::Entity& input, + cudaStream_t stream, const char* output_name, + const char* input_name) { + + // Run the image normalization operation + DEFINE_NORMALIZE_C1(cvcore::tensor_ops::ImageType::Y_U8, + cvcore::tensor_ops::ImageType::Y_F32); + DEFINE_NORMALIZE_C1(cvcore::tensor_ops::ImageType::Y_U16, + cvcore::tensor_ops::ImageType::Y_F32); + DEFINE_NORMALIZE_C1(cvcore::tensor_ops::ImageType::Y_F32, + cvcore::tensor_ops::ImageType::Y_F32); + DEFINE_NORMALIZE_C3(cvcore::tensor_ops::ImageType::RGB_U8, + cvcore::tensor_ops::ImageType::RGB_F32); + DEFINE_NORMALIZE_C3(cvcore::tensor_ops::ImageType::RGB_U16, + cvcore::tensor_ops::ImageType::RGB_F32); + DEFINE_NORMALIZE_C3(cvcore::tensor_ops::ImageType::RGB_F32, + cvcore::tensor_ops::ImageType::RGB_F32); + DEFINE_NORMALIZE_C3(cvcore::tensor_ops::ImageType::BGR_U8, + cvcore::tensor_ops::ImageType::BGR_F32); + DEFINE_NORMALIZE_C3(cvcore::tensor_ops::ImageType::BGR_U16, + cvcore::tensor_ops::ImageType::BGR_F32); + DEFINE_NORMALIZE_C3(cvcore::tensor_ops::ImageType::BGR_F32, + cvcore::tensor_ops::ImageType::BGR_F32); + + // Return error code for unsupported type + GXF_LOG_ERROR("invalid input/output type for image normalize."); + return GXF_FAILURE; +} + +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/Normalize.hpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Normalize.hpp similarity index 77% rename from isaac_ros_image_proc/gxf/tensorops/Normalize.hpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Normalize.hpp index efb735a..bf838d2 100644 --- a/isaac_ros_image_proc/gxf/tensorops/Normalize.hpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Normalize.hpp @@ -14,34 +14,33 @@ // limitations under the License. // // SPDX-License-Identifier: Apache-2.0 -#ifndef NVIDIA_CVCORE_NORMALIZE_HPP -#define NVIDIA_CVCORE_NORMALIZE_HPP +#pragma once -#include "TensorOperator.hpp" +#include +#include "extensions/tensorops/components/TensorOperator.hpp" namespace nvidia { -namespace cvcore { +namespace isaac { namespace tensor_ops { // Normalization operator. class Normalize : public TensorOperator { -public: + public: virtual ~Normalize() {} gxf_result_t registerInterface(gxf::Registrar* registrar) override; -private: - gxf::Expected doInferOutputInfo(gxf::Entity& input) override final; + private: + gxf::Expected doInferOutputInfo(gxf::Entity& input) override; gxf_result_t doUpdateCameraMessage(gxf::Handle& output, - gxf::Handle& input) override final; - gxf_result_t doExecute(gxf::Entity& output, gxf::Entity& input, cudaStream_t stream, const char* output_name, - const char* input_name) override final; + gxf::Handle& input) override; + gxf_result_t doExecute(gxf::Entity& output, gxf::Entity& input, + cudaStream_t stream, const char* output_name, + const char* input_name) override; gxf::Parameter> scales_; gxf::Parameter> offsets_; }; -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia - -#endif +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/Reshape.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Reshape.cpp similarity index 67% rename from isaac_ros_image_proc/gxf/tensorops/Reshape.cpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Reshape.cpp index 322b843..bdce209 100644 --- a/isaac_ros_image_proc/gxf/tensorops/Reshape.cpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Reshape.cpp @@ -16,23 +16,36 @@ // SPDX-License-Identifier: Apache-2.0 #include "Reshape.hpp" +#include +#include + namespace nvidia { -namespace cvcore { +namespace isaac { namespace tensor_ops { gxf_result_t Reshape::registerInterface(gxf::Registrar* registrar) { gxf::Expected result; - result &= registrar->parameter(output_shape_, "output_shape"); - result &= registrar->parameter(receiver_, "receiver"); - result &= registrar->parameter(transmitter_, "transmitter"); - result &= registrar->parameter(pool_, "pool"); - result &= registrar->parameter(input_adapter_, "input_adapter"); - result &= registrar->parameter(output_adapter_, "output_adapter"); - result &= registrar->parameter(input_name_, "input_name", "input name", "input tensor name", - gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); - result &= registrar->parameter(output_name_, "output_name", "output name", "output tensor name", - gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + result &= registrar->parameter( + output_shape_, "output_shape"); + result &= registrar->parameter( + receiver_, "receiver"); + result &= registrar->parameter( + transmitter_, "transmitter"); + result &= registrar->parameter( + pool_, "pool"); + result &= registrar->parameter( + input_adapter_, "input_adapter"); + result &= registrar->parameter( + output_adapter_, "output_adapter"); + result &= registrar->parameter( + input_name_, "input_name", + "input name", "input tensor name", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + result &= registrar->parameter( + output_name_, "output_name", + "output name", "output tensor name", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); return gxf::ToResultCode(result); } @@ -43,9 +56,9 @@ gxf_result_t Reshape::doUpdateCameraMessage(gxf::Handle& outpu return GXF_SUCCESS; } -gxf_result_t Reshape::doExecute(gxf::Entity& output, gxf::Entity& input, cudaStream_t stream, const char* output_name, - const char* input_name) { - GXF_LOG_INFO("execute reshape."); +gxf_result_t Reshape::doExecute(gxf::Entity& output, gxf::Entity& input, + cudaStream_t stream, const char* output_name, + const char* input_name) { auto input_tensor = input.get(input_name); if (!input_tensor) { @@ -71,7 +84,8 @@ gxf_result_t Reshape::doExecute(gxf::Entity& output, gxf::Entity& input, cudaStr } auto result = output_tensor.value()->reshapeCustom( - output_shape, input_tensor.value()->element_type(), gxf::PrimitiveTypeSize(input_tensor.value()->element_type()), + output_shape, input_tensor.value()->element_type(), + gxf::PrimitiveTypeSize(input_tensor.value()->element_type()), gxf::Unexpected{GXF_UNINITIALIZED_VALUE}, input_tensor.value()->storage_type(), pool_.get()); if (!result) { @@ -81,18 +95,21 @@ gxf_result_t Reshape::doExecute(gxf::Entity& output, gxf::Entity& input, cudaStr // Simply copy the memory if (input_tensor.value()->storage_type() == gxf::MemoryStorageType::kDevice) { - cudaError_t error = cudaMemcpyAsync(output_tensor.value()->pointer(), input_tensor.value()->pointer(), - input_tensor.value()->size(), cudaMemcpyDeviceToDevice, stream); + cudaError_t error = cudaMemcpyAsync(output_tensor.value()->pointer(), + input_tensor.value()->pointer(), + input_tensor.value()->size(), + cudaMemcpyDeviceToDevice, stream); if (error != cudaSuccess) { GXF_LOG_ERROR("cudaMemcpyAsync returned error code"); return GXF_FAILURE; } } else { - memcpy(output_tensor.value()->pointer(), input_tensor.value()->pointer(), input_tensor.value()->size()); + memcpy(output_tensor.value()->pointer(), input_tensor.value()->pointer(), + input_tensor.value()->size()); } return GXF_SUCCESS; } -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/Reshape.hpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Reshape.hpp similarity index 78% rename from isaac_ros_image_proc/gxf/tensorops/Reshape.hpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Reshape.hpp index c6f1c6f..8d8b717 100644 --- a/isaac_ros_image_proc/gxf/tensorops/Reshape.hpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Reshape.hpp @@ -14,36 +14,36 @@ // limitations under the License. // // SPDX-License-Identifier: Apache-2.0 -#ifndef NVIDIA_CVCORE_RESHAPE_HPP -#define NVIDIA_CVCORE_RESHAPE_HPP +#pragma once -#include "TensorOperator.hpp" +#include +#include +#include "extensions/tensorops/components/TensorOperator.hpp" namespace nvidia { -namespace cvcore { +namespace isaac { namespace tensor_ops { // Reshaping operator (only valid for gxf::Tensor). class Reshape : public TensorOperator { -public: + public: virtual ~Reshape() {} gxf_result_t registerInterface(gxf::Registrar* registrar) override; -private: - gxf::Expected doInferOutputInfo(gxf::Entity& input) override final { + private: + gxf::Expected doInferOutputInfo(gxf::Entity& input) override { no_op_ = false; return ImageInfo{}; }; gxf_result_t doUpdateCameraMessage(gxf::Handle& output, - gxf::Handle& input) override final; - gxf_result_t doExecute(gxf::Entity& output, gxf::Entity& input, cudaStream_t stream, const char* output_name, - const char* input_name) override final; + gxf::Handle& input) override; + gxf_result_t doExecute(gxf::Entity& output, gxf::Entity& input, + cudaStream_t stream, const char* output_name, + const char* input_name) override; gxf::Parameter> output_shape_; }; -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia - -#endif +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Resize.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Resize.cpp new file mode 100644 index 0000000..6fd3cc9 --- /dev/null +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Resize.cpp @@ -0,0 +1,223 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 +#include "Resize.hpp" + +namespace nvidia { +namespace isaac { +namespace tensor_ops { + +namespace detail { + +template +gxf_result_t ResizeImpl(gxf::Entity& output, gxf::Entity& input, + const ImageInfo& output_info, + const ImageInfo& input_info, const char* output_name, const char* input_name, + gxf::Handle output_adapter, gxf::Handle input_adapter, + gxf::Handle allocator, bool keep_aspect_ratio, + cvcore::tensor_ops::InterpolationType interp_type, cudaStream_t stream) { + auto input_image = input_adapter->WrapImageFromMessage(input, input_name); + if (!input_image) { + return GXF_FAILURE; + } + + auto error = output_adapter->AddImageToMessage(output, output_info.width, + output_info.height, allocator, + output_info.is_cpu, output_name); + if (error != GXF_SUCCESS) { + return GXF_FAILURE; + } + + auto output_image = output_adapter->WrapImageFromMessage(output, output_name); + if (!output_image) { + return GXF_FAILURE; + } + cvcore::tensor_ops::Resize(output_image.value(), input_image.value(), + keep_aspect_ratio, interp_type, stream); + return GXF_SUCCESS; +} + +template +gxf_result_t ResizeStreamImpl(gxf::Entity& output, + gxf::Entity& input, const ImageInfo& output_info, + const ImageInfo& input_info, const char* output_name, const char* input_name, + gxf::Handle stream, gxf::Handle output_adapter, + gxf::Handle input_adapter, gxf::Handle allocator, + cvcore::tensor_ops::InterpolationType interp_type, + cvcore::tensor_ops::BorderType border_type) { + auto input_image = input_adapter->WrapImageFromMessage(input, input_name); + if (!input_image) { + return GXF_FAILURE; + } + + auto error = output_adapter->AddImageToMessage(output, + output_info.width, output_info.height, allocator, + output_info.is_cpu, output_name); + if (error != GXF_SUCCESS) { + return GXF_FAILURE; + } + + auto output_image = output_adapter->WrapImageFromMessage(output, output_name); + if (!output_image) { + return GXF_FAILURE; + } + + auto err_code = stream->getStream()->Resize(output_image.value(), + input_image.value(), interp_type, border_type); + if (err_code != cvcore::tensor_ops::make_error_condition( + cvcore::tensor_ops::ErrorCode::SUCCESS)) { + GXF_LOG_ERROR("resize operation failed."); + return GXF_FAILURE; + } + + return GXF_SUCCESS; +} + +} // namespace detail + +template +gxf_result_t ResizeBase::registerInterface(gxf::Registrar* registrar) { + gxf::Expected result; + + result &= registrar->parameter( + output_width_, "output_width"); + result &= registrar->parameter( + output_height_, "output_height"); + result &= registrar->parameter( + interp_type_, "interp_type"); + result &= registrar->parameter( + border_type_, "border_type"); + result &= registrar->parameter( + keep_aspect_ratio_, "keep_aspect_ratio"); + result &= registrar->parameter( + receiver_, "receiver"); + result &= registrar->parameter( + transmitter_, "transmitter"); + result &= registrar->parameter( + pool_, "pool"); + result &= registrar->parameter( + stream_, "stream", "tensor stream", "tensor stream object", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + result &= registrar->parameter( + stream_pool_, "stream_pool", "cuda stream pool", + "cuda stream pool object", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + result &= registrar->parameter( + input_adapter_, "input_adapter"); + result &= registrar->parameter( + output_adapter_, "output_adapter"); + result &= registrar->parameter( + input_name_, "input_name", + "input name", "input tensor name", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + result &= registrar->parameter( + output_name_, "output_name", + "output name", "output tensor name", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + + return gxf::ToResultCode(result); +} + +template +gxf::Expected ResizeBase::doInferOutputInfo(gxf::Entity& input) { + // Check if no-op is needed + no_op_ = output_width_.get() == input_info_.width && output_height_.get() == input_info_.height; + return ImageInfo{input_info_.type, output_width_.get(), output_height_.get(), input_info_.is_cpu}; +} + +template +gxf_result_t ResizeBase::doUpdateCameraMessage( + gxf::Handle& output, + gxf::Handle& input) { + *output = GetScaledCameraModel(*input, output_info_.width, + output_info_.height, keep_aspect_ratio_.get()).value(); + return GXF_SUCCESS; +} + +#define DEFINE_RESIZE(INPUT_TYPE) \ + if (input_info_.type == INPUT_TYPE) { \ + return detail::ResizeImpl(output, input, output_info_, \ + input_info_, output_name, input_name, \ + output_adapter_.get(), input_adapter_.get(), pool_.get(), \ + keep_aspect_ratio_.get(), interp.value(), stream); \ + } + +#define DEFINE_STREAM_RESIZE(INPUT_TYPE) \ + if (input_info_.type == INPUT_TYPE) { \ + return detail::ResizeStreamImpl(output, input, output_info_, \ + input_info_, output_name, input_name, \ + stream_.try_get().value(), output_adapter_.get(), \ + input_adapter_.get(), pool_.get(), interp.value(), border.value()); \ + } + +template<> +gxf_result_t ResizeBase::doExecute(gxf::Entity& output, + gxf::Entity& input, cudaStream_t stream, + const char* output_name, const char* input_name) { + // Check if interpolation type is valid + auto interp = GetInterpolationType(interp_type_); + if (!interp) { + return interp.error(); + } + auto border = GetBorderType(border_type_); + if (!border) { + return border.error(); + } + + // Run the image resizing operation + DEFINE_STREAM_RESIZE(cvcore::tensor_ops::ImageType::RGB_U8); + DEFINE_STREAM_RESIZE(cvcore::tensor_ops::ImageType::BGR_U8); + DEFINE_STREAM_RESIZE(cvcore::tensor_ops::ImageType::NV12); + DEFINE_STREAM_RESIZE(cvcore::tensor_ops::ImageType::NV24); + DEFINE_STREAM_RESIZE(cvcore::tensor_ops::ImageType::Y_U8); + + // Return error code for unsupported type + GXF_LOG_ERROR("invalid input/output type for image resize."); + return GXF_FAILURE; +} + +template<> +gxf_result_t ResizeBase::doExecute(gxf::Entity& output, + gxf::Entity& input, cudaStream_t stream, + const char* output_name, const char* input_name) { + // Check if interpolation type is valid + auto interp = GetInterpolationType(interp_type_); + if (!interp) { + return interp.error(); + } + + // Run the image resizing operation + DEFINE_RESIZE(cvcore::tensor_ops::ImageType::Y_U8); + DEFINE_RESIZE(cvcore::tensor_ops::ImageType::Y_U16); + DEFINE_RESIZE(cvcore::tensor_ops::ImageType::Y_F32); + DEFINE_RESIZE(cvcore::tensor_ops::ImageType::RGB_U8); + DEFINE_RESIZE(cvcore::tensor_ops::ImageType::RGB_U16); + DEFINE_RESIZE(cvcore::tensor_ops::ImageType::RGB_F32); + DEFINE_RESIZE(cvcore::tensor_ops::ImageType::BGR_U8); + DEFINE_RESIZE(cvcore::tensor_ops::ImageType::BGR_U16); + DEFINE_RESIZE(cvcore::tensor_ops::ImageType::BGR_F32); + + // Return error code for unsupported type + GXF_LOG_ERROR("invalid input/output type for image resize."); + return GXF_FAILURE; +} + +template class ResizeBase; +template class ResizeBase; + +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/Resize.hpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Resize.hpp similarity index 77% rename from isaac_ros_image_proc/gxf/tensorops/Resize.hpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Resize.hpp index 6771e6e..54bbf83 100644 --- a/isaac_ros_image_proc/gxf/tensorops/Resize.hpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Resize.hpp @@ -14,29 +14,30 @@ // limitations under the License. // // SPDX-License-Identifier: Apache-2.0 -#ifndef NVIDIA_CVCORE_RESIZE_HPP -#define NVIDIA_CVCORE_RESIZE_HPP +#pragma once -#include "TensorOperator.hpp" +#include +#include "extensions/tensorops/components/TensorOperator.hpp" namespace nvidia { -namespace cvcore { +namespace isaac { namespace tensor_ops { // Resizing operator. template class ResizeBase : public TensorOperator { -public: + public: virtual ~ResizeBase() {} gxf_result_t registerInterface(gxf::Registrar* registrar) override; -private: - gxf::Expected doInferOutputInfo(gxf::Entity& input) override final; + private: + gxf::Expected doInferOutputInfo(gxf::Entity& input) override; gxf_result_t doUpdateCameraMessage(gxf::Handle& output, - gxf::Handle& input) override final; - gxf_result_t doExecute(gxf::Entity& output, gxf::Entity& input, cudaStream_t stream, const char* output_name, - const char* input_name) override final; + gxf::Handle& input) override; + gxf_result_t doExecute(gxf::Entity& output, gxf::Entity& input, + cudaStream_t stream, const char* output_name, + const char* input_name) override; gxf::Parameter output_width_; gxf::Parameter output_height_; @@ -48,8 +49,6 @@ class ResizeBase : public TensorOperator { class Resize : public ResizeBase {}; class StreamResize : public ResizeBase {}; -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia - -#endif +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/TensorOperator.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/TensorOperator.cpp similarity index 70% rename from isaac_ros_image_proc/gxf/tensorops/TensorOperator.cpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/TensorOperator.cpp index 294fb06..572751d 100644 --- a/isaac_ros_image_proc/gxf/tensorops/TensorOperator.cpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/TensorOperator.cpp @@ -16,10 +16,11 @@ // SPDX-License-Identifier: Apache-2.0 #include "TensorOperator.hpp" +#include #include "gxf/std/timestamp.hpp" namespace nvidia { -namespace cvcore { +namespace isaac { namespace tensor_ops { namespace detail { @@ -53,7 +54,7 @@ gxf_result_t RecordCudaEvent(gxf::Entity& message, gxf::Handle& // Record the event // Can define []() { GXF_LOG_DEBUG("tensorops event synced"); } as callback func for debug purpose ret = stream->record(event.event().value(), - [event = cuda_event, entity = message.clone().value()](auto) { cudaEventDestroy(event); }); + [event = cuda_event, entity = message.clone().value()](auto) { cudaEventDestroy(event); }); if (!ret) { GXF_LOG_ERROR("record event failed"); return ret.error(); @@ -65,7 +66,7 @@ template gxf_result_t RerouteMessage(gxf::Entity& output, gxf::Entity& input, std::function, gxf::Handle)> func, const char* name = nullptr) { - auto maybe_component = input.get(); + auto maybe_component = (name != nullptr ? input.get(name) : input.get()); if (maybe_component) { auto output_component = output.add(name != nullptr ? name : maybe_component.value().name()); if (!output_component) { @@ -77,7 +78,7 @@ gxf_result_t RerouteMessage(gxf::Entity& output, gxf::Entity& input, return GXF_SUCCESS; } -} // namespace detail +} // namespace detail gxf_result_t TensorOperator::inferOutputInfo(gxf::Entity& input) { const char* input_name = input_name_.try_get() ? input_name_.try_get().value().c_str() : nullptr; @@ -94,14 +95,27 @@ gxf_result_t TensorOperator::inferOutputInfo(gxf::Entity& input) { return GXF_SUCCESS; } +gxf_result_t TensorOperator::doForwardTargetCamera(gxf::Expected input_message, + gxf::Expected output_message) { + return detail::RerouteMessage( + output_message.value(), input_message.value(), + [](gxf::Handle output, gxf::Handle input) { + *output = *input; + return GXF_SUCCESS; + }, + "target_camera"); +} + gxf_result_t TensorOperator::updateCameraMessage(gxf::Handle& output, gxf::Handle& input) { return doUpdateCameraMessage(output, input); } gxf_result_t TensorOperator::execute(gxf::Entity& output, gxf::Entity& input, cudaStream_t stream) { - const char* output_name = output_name_.try_get() ? output_name_.try_get().value().c_str() : nullptr; - const char* input_name = input_name_.try_get() ? input_name_.try_get().value().c_str() : nullptr; + const char* output_name = output_name_.try_get() ? + output_name_.try_get().value().c_str() : nullptr; + const char* input_name = input_name_.try_get() ? + input_name_.try_get().value().c_str() : nullptr; return doExecute(output, input, stream, output_name, input_name); } @@ -122,6 +136,10 @@ gxf_result_t TensorOperator::start() { return GXF_SUCCESS; } +gxf_result_t TensorOperator::stop() { + return GXF_SUCCESS; +} + gxf_result_t TensorOperator::tick() { // Receiving the data auto input_message = receiver_->receive(); @@ -147,15 +165,36 @@ gxf_result_t TensorOperator::tick() { // Pass through timestamp if presented in input message error = detail::RerouteMessage(output_message.value(), input_message.value(), - [](gxf::Handle output, gxf::Handle input) { - *output = *input; - return GXF_SUCCESS; - }); + [](gxf::Handle output, gxf::Handle input) { + *output = *input; + return GXF_SUCCESS; + }); + if (error != GXF_SUCCESS) { + return error; + } + // Pass through Named timestamp if presented in input message + auto maybe_named_timestamp = input_message->get("timestamp"); + if (maybe_named_timestamp) { + auto output_named_timestamp = output_message->add("timestamp"); + if (!output_named_timestamp) { + GXF_LOG_ERROR("add output timestamp failed."); + return gxf::ToResultCode(output_named_timestamp); + } + *output_named_timestamp.value() = *maybe_named_timestamp.value(); + } + // Pass through Sequence number if presented in input message + error = + detail::RerouteMessage(output_message.value(), input_message.value(), + [](gxf::Handle output, gxf::Handle input) { + *output = *input; + return GXF_SUCCESS; + }, + "sequence_number"); if (error != GXF_SUCCESS) { return error; } // Pass through cudaStreamId or create a new cuda stream for NPP backend only - cudaStream_t cuda_stream = 0; // default stream + cudaStream_t cuda_stream = 0; // default stream if (!stream_.try_get()) { // Allocate new CudaStream if StreamPool attached if (stream_pool_.try_get()) { @@ -167,7 +206,8 @@ gxf_result_t TensorOperator::tick() { auto input_stream_id = input_message.value().get(); if (input_stream_id) { auto stream = - gxf::Handle::Create(input_stream_id.value().context(), input_stream_id.value()->stream_cid); + gxf::Handle::Create(input_stream_id.value().context(), + input_stream_id.value()->stream_cid); if (!stream) { GXF_LOG_ERROR("create cudastream from cid failed."); return GXF_FAILURE; @@ -209,18 +249,39 @@ gxf_result_t TensorOperator::tick() { [this](gxf::Handle output, gxf::Handle input) { return updateCameraMessage(output, input); }, - "camera"); + "intrinsics"); if (error != GXF_SUCCESS) { return error; } - // Pass through pose3d message if necessary + + // Pass through "target_camera" if necessary. If "target_camera" does not need to be + // forwarded (e.g. Undistort module), the function doForwardTargetCamera can be overwritten + // accordingly + error = doForwardTargetCamera(input_message, output_message); + if (error != GXF_SUCCESS) { + return error; + } + + // Pass through extrinsics message if necessary + error = detail::RerouteMessage( + output_message.value(), input_message.value(), + [](gxf::Handle output, gxf::Handle input) { + *output = *input; + return GXF_SUCCESS; + }, + "extrinsics"); + if (error != GXF_SUCCESS) { + return error; + } + + // Pass through target extrinsics message if necessary error = detail::RerouteMessage( output_message.value(), input_message.value(), [](gxf::Handle output, gxf::Handle input) { *output = *input; return GXF_SUCCESS; }, - "pose"); + "target_extrinsics_delta"); if (error != GXF_SUCCESS) { return error; } @@ -230,6 +291,6 @@ gxf_result_t TensorOperator::tick() { return GXF_SUCCESS; } -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/TensorOperator.hpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/TensorOperator.hpp similarity index 75% rename from isaac_ros_image_proc/gxf/tensorops/TensorOperator.hpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/TensorOperator.hpp index 4c65d47..8c79ed6 100644 --- a/isaac_ros_image_proc/gxf/tensorops/TensorOperator.hpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/TensorOperator.hpp @@ -14,12 +14,12 @@ // limitations under the License. // // SPDX-License-Identifier: Apache-2.0 -#ifndef NVIDIA_CVCORE_TENSOR_OPERATOR_HPP -#define NVIDIA_CVCORE_TENSOR_OPERATOR_HPP +#pragma once -#include "ImageAdapter.hpp" -#include "ImageUtils.hpp" -#include "TensorStream.hpp" +#include +#include "extensions/tensorops/components/ImageAdapter.hpp" +#include "extensions/tensorops/components/ImageUtils.hpp" +#include "extensions/tensorops/components/TensorStream.hpp" #include "gxf/cuda/cuda_stream.hpp" #include "gxf/cuda/cuda_stream_id.hpp" @@ -31,22 +31,23 @@ #include "gxf/std/tensor.hpp" #include "gxf/std/transmitter.hpp" -#include "cv/core/Image.h" -#include "cv/core/Tensor.h" -#include "cv/tensor_ops/ImageUtils.h" +#include "extensions/tensorops/core/Image.h" +#include "extensions/tensorops/core/ImageUtils.h" +#include "extensions/tensorops/core/Tensor.h" namespace nvidia { -namespace cvcore { +namespace isaac { namespace tensor_ops { // Base class for all tensor_ops operators class TensorOperator : public gxf::Codelet { -public: + public: virtual ~TensorOperator() = default; gxf_result_t inferOutputInfo(gxf::Entity& input); - gxf_result_t updateCameraMessage(gxf::Handle& output, gxf::Handle& input); + gxf_result_t updateCameraMessage(gxf::Handle& output, + gxf::Handle& input); gxf_result_t execute(gxf::Entity& output, gxf::Entity& input, cudaStream_t stream); @@ -54,11 +55,9 @@ class TensorOperator : public gxf::Codelet { gxf_result_t tick() override; - virtual gxf_result_t stop() override { - return GXF_SUCCESS; - } + gxf_result_t stop() override; -protected: + protected: gxf::Parameter> receiver_; gxf::Parameter> transmitter_; gxf::Parameter> pool_; @@ -76,20 +75,21 @@ class TensorOperator : public gxf::Codelet { // Whether to skip the operation(by default is false) bool no_op_ = false; -private: + private: virtual gxf::Expected doInferOutputInfo(gxf::Entity& input) = 0; virtual gxf_result_t doUpdateCameraMessage(gxf::Handle& output, gxf::Handle& input) = 0; - virtual gxf_result_t doExecute(gxf::Entity& output, gxf::Entity& input, cudaStream_t stream, const char* output_name, - const char* input_name) = 0; + virtual gxf_result_t doForwardTargetCamera(gxf::Expected input_message, + gxf::Expected output_message); + + virtual gxf_result_t doExecute(gxf::Entity& output, gxf::Entity& input, + cudaStream_t stream, const char* output_name, const char* input_name) = 0; gxf::Handle cuda_stream_ptr_ = nullptr; }; -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia - -#endif +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/TensorStream.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/TensorStream.cpp similarity index 59% rename from isaac_ros_image_proc/gxf/tensorops/TensorStream.cpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/TensorStream.cpp index 2f825cf..6f856d9 100644 --- a/isaac_ros_image_proc/gxf/tensorops/TensorStream.cpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/TensorStream.cpp @@ -17,50 +17,51 @@ #include "TensorStream.hpp" -#include "cv/core/ComputeEngine.h" -#include "cv/tensor_ops/TensorOperators.h" +#include +#include "extensions/tensorops/core/ComputeEngine.h" +#include "extensions/tensorops/core/TensorOperators.h" namespace nvidia { -namespace cvcore { +namespace isaac { namespace tensor_ops { namespace detail { -gxf::Expected<::cvcore::tensor_ops::TensorBackend> GetContextType(const std::string& type) { +gxf::Expected GetContextType(const std::string& type) { if (type == "NPP") { - return ::cvcore::tensor_ops::TensorBackend::NPP; + return cvcore::tensor_ops::TensorBackend::NPP; } else if (type == "VPI") { - return ::cvcore::tensor_ops::TensorBackend::VPI; + return cvcore::tensor_ops::TensorBackend::VPI; } else if (type == "DALI") { - return ::cvcore::tensor_ops::TensorBackend::DALI; + return cvcore::tensor_ops::TensorBackend::DALI; } else { return gxf::Unexpected{GXF_FAILURE}; } } -gxf::Expected<::cvcore::ComputeEngine> GetComputeEngineType(const std::string& type) { +gxf::Expected GetComputeEngineType(const std::string& type) { if (type == "UNKNOWN") { - return ::cvcore::ComputeEngine::UNKNOWN; + return cvcore::tensor_ops::ComputeEngine::UNKNOWN; } else if (type == "CPU") { - return ::cvcore::ComputeEngine::CPU; + return cvcore::tensor_ops::ComputeEngine::CPU; } else if (type == "PVA") { - return ::cvcore::ComputeEngine::PVA; + return cvcore::tensor_ops::ComputeEngine::PVA; } else if (type == "VIC") { - return ::cvcore::ComputeEngine::VIC; + return cvcore::tensor_ops::ComputeEngine::VIC; } else if (type == "NVENC") { - return ::cvcore::ComputeEngine::NVENC; + return cvcore::tensor_ops::ComputeEngine::NVENC; } else if (type == "GPU") { - return ::cvcore::ComputeEngine::GPU; + return cvcore::tensor_ops::ComputeEngine::GPU; } else if (type == "DLA") { - return ::cvcore::ComputeEngine::DLA; + return cvcore::tensor_ops::ComputeEngine::DLA; } else if (type == "COMPUTE_FAULT") { - return ::cvcore::ComputeEngine::COMPUTE_FAULT; + return cvcore::tensor_ops::ComputeEngine::COMPUTE_FAULT; } else { return gxf::Unexpected{GXF_FAILURE}; } } -} // namespace detail +} // namespace detail gxf_result_t TensorStream::registerInterface(gxf::Registrar* registrar) { gxf::Expected result; @@ -78,12 +79,13 @@ gxf_result_t TensorStream::initialize() { GXF_LOG_ERROR("unknown backend type."); return GXF_FAILURE; } - if (!::cvcore::tensor_ops::TensorContextFactory::IsBackendSupported(backend_type.value())) { - GXF_LOG_ERROR("unsupported context type."); + if (!cvcore::tensor_ops::TensorContextFactory::IsBackendSupported(backend_type.value())) { + GXF_LOG_ERROR("unsupported context type:%d", backend_type.value()); return GXF_FAILURE; } - auto err_code = ::cvcore::tensor_ops::TensorContextFactory::CreateContext(context_, backend_type.value()); - if (err_code != ::cvcore::make_error_code(::cvcore::ErrorCode::SUCCESS)) { + auto err_code = cvcore::tensor_ops::TensorContextFactory::CreateContext( + context_, backend_type.value()); + if (err_code != cvcore::tensor_ops::make_error_code(cvcore::tensor_ops::ErrorCode::SUCCESS)) { GXF_LOG_ERROR("tensor context creation failed."); return GXF_FAILURE; } @@ -98,7 +100,7 @@ gxf_result_t TensorStream::initialize() { return GXF_FAILURE; } err_code = context_->CreateStream(stream_, engine_type.value()); - if (err_code != ::cvcore::make_error_code(::cvcore::ErrorCode::SUCCESS)) { + if (err_code != cvcore::tensor_ops::make_error_code(cvcore::tensor_ops::ErrorCode::SUCCESS)) { GXF_LOG_ERROR("tensor stream creation failed."); return GXF_FAILURE; } @@ -107,18 +109,18 @@ gxf_result_t TensorStream::initialize() { gxf_result_t TensorStream::deinitialize() { auto err_code = context_->DestroyStream(stream_); - if (err_code != ::cvcore::make_error_code(::cvcore::ErrorCode::SUCCESS)) { + if (err_code != cvcore::tensor_ops::make_error_code(cvcore::tensor_ops::ErrorCode::SUCCESS)) { GXF_LOG_ERROR("tensor stream destroy failed."); return GXF_FAILURE; } - err_code = ::cvcore::tensor_ops::TensorContextFactory::DestroyContext(context_); - if (err_code != ::cvcore::make_error_code(::cvcore::ErrorCode::SUCCESS)) { + err_code = cvcore::tensor_ops::TensorContextFactory::DestroyContext(context_); + if (err_code != cvcore::tensor_ops::make_error_code(cvcore::tensor_ops::ErrorCode::SUCCESS)) { GXF_LOG_ERROR("tensor context destroy failed."); return GXF_FAILURE; } return GXF_SUCCESS; } -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/TensorStream.hpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/TensorStream.hpp similarity index 71% rename from isaac_ros_image_proc/gxf/tensorops/TensorStream.hpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/TensorStream.hpp index 710e11f..f412454 100644 --- a/isaac_ros_image_proc/gxf/tensorops/TensorStream.hpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/TensorStream.hpp @@ -14,22 +14,21 @@ // limitations under the License. // // SPDX-License-Identifier: Apache-2.0 -#ifndef NVIDIA_CVCORE_TENSOR_STREAM_HPP -#define NVIDIA_CVCORE_TENSOR_STREAM_HPP +#pragma once +#include +#include "extensions/tensorops/core/ITensorOperatorContext.h" +#include "extensions/tensorops/core/ITensorOperatorStream.h" #include "gxf/core/component.hpp" #include "gxf/std/parameter_parser_std.hpp" -#include "cv/tensor_ops/ITensorOperatorContext.h" -#include "cv/tensor_ops/ITensorOperatorStream.h" - namespace nvidia { -namespace cvcore { +namespace isaac { namespace tensor_ops { // Wrapper of CVCORE ITensorOperatorStream/ITensorOperatorContext class TensorStream : public gxf::Component { -public: + public: virtual ~TensorStream() = default; TensorStream() = default; @@ -37,23 +36,21 @@ class TensorStream : public gxf::Component { gxf_result_t initialize() override; gxf_result_t deinitialize() override; - ::cvcore::tensor_ops::TensorOperatorContext getContext() const { + cvcore::tensor_ops::TensorOperatorContext getContext() const { return context_; } - ::cvcore::tensor_ops::TensorOperatorStream getStream() const { + cvcore::tensor_ops::TensorOperatorStream getStream() const { return stream_; } -private: + private: gxf::Parameter backend_type_; gxf::Parameter engine_type_; - ::cvcore::tensor_ops::TensorOperatorContext context_; - ::cvcore::tensor_ops::TensorOperatorStream stream_; + cvcore::tensor_ops::TensorOperatorContext context_; + cvcore::tensor_ops::TensorOperatorStream stream_; }; -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia - -#endif +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Undistort.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Undistort.cpp new file mode 100644 index 0000000..1cc8d3e --- /dev/null +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Undistort.cpp @@ -0,0 +1,336 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 +#include "Undistort.hpp" + +#include +#include + +#include "extensions/tensorops/components/ImageUtils.hpp" +#include "gxf/multimedia/camera.hpp" + + +namespace nvidia { +namespace isaac { +namespace tensor_ops { + +namespace detail { + +template +gxf_result_t UndistortImpl(gxf::Entity& output, + gxf::Entity& input, const ImageInfo& output_info, + const ImageInfo& input_info, const char* output_name, const char* input_name, + gxf::Handle stream, gxf::Handle output_adapter, + gxf::Handle input_adapter, gxf::Handle allocator, + cvcore::tensor_ops::ImageWarp warp, cvcore::tensor_ops::InterpolationType interp_type, + cvcore::tensor_ops::BorderType border_type) { + auto input_image = input_adapter->WrapImageFromMessage(input, input_name); + if (!input_image) { + return GXF_FAILURE; + } + + auto error = output_adapter->AddImageToMessage(output, output_info.width, + output_info.height, allocator, output_info.is_cpu, "frame"); + if (error != GXF_SUCCESS) { + return GXF_FAILURE; + } + + auto output_image = output_adapter->WrapImageFromMessage(output, "frame"); + if (!output_image) { + return GXF_FAILURE; + } + + auto err_code = stream->getStream()->Remap(output_image.value(), + input_image.value(), warp, interp_type, border_type); + if (err_code != cvcore::tensor_ops::make_error_condition( + cvcore::tensor_ops::ErrorCode::SUCCESS)) { + GXF_LOG_ERROR("undistort operation failed."); + return GXF_FAILURE; + } + + return GXF_SUCCESS; +} + +gxf::Expected GetIntrinsicsFromMessage( + gxf::Handle& camera_model) { + return cvcore::tensor_ops::CameraIntrinsics( + camera_model->focal_length.x, camera_model->focal_length.y, + camera_model->principal_point.x, camera_model->principal_point.y, + camera_model->skew_value); +} + +gxf::Expected GetExtrinsicsFromMessage( + gxf::Handle& pose) { + float raw_matrix[3][4]; + for (size_t i = 0; i < 9; i++) { + raw_matrix[i / 3][i % 3] = pose->rotation[i]; + } + for (size_t i = 0; i < 3; i++) { + raw_matrix[i][3] = pose->translation[i]; + } + return cvcore::tensor_ops::CameraExtrinsics(raw_matrix); +} + +gxf::Expected GetDistortionsFromMessage( + gxf::Handle& camera_model) { + auto distortion_type = GetCameraDistortionType(camera_model->distortion_type); + if (!distortion_type) { + return gxf::Unexpected{GXF_FAILURE}; + } + auto distortion_model = cvcore::tensor_ops::CameraDistortionModel(); + for (size_t i = 0; i < 8; i++) { + distortion_model.coefficients[i] = camera_model->distortion_coefficients[i]; + } + distortion_model.type = distortion_type.value(); + return distortion_model; +} + +} // namespace detail + +gxf_result_t UndistortBase::start() { + // Load grid object + image_grid_.numHorizRegions = regions_width_.get().size(); + image_grid_.numVertRegions = regions_height_.get().size(); + if (regions_width_.get().size() != horizontal_intervals_.get().size() || + regions_height_.get().size() != vertical_intervals_.get().size()) { + GXF_LOG_ERROR("invalid image grid."); + return GXF_FAILURE; + } + std::copy(regions_width_.get().begin(), regions_width_.get().end(), + image_grid_.regionWidth.begin()); + std::copy(regions_height_.get().begin(), regions_height_.get().end(), + image_grid_.regionHeight.begin()); + std::copy(horizontal_intervals_.get().begin(), horizontal_intervals_.get().end(), + image_grid_.horizInterval.begin()); + std::copy(vertical_intervals_.get().begin(), vertical_intervals_.get().end(), + image_grid_.vertInterval.begin()); + output_shape_.x = static_cast( + std::accumulate(image_grid_.regionWidth.begin(), image_grid_.regionWidth.end(), 0)); + output_shape_.y = static_cast( + std::accumulate(image_grid_.regionHeight.begin(), image_grid_.regionHeight.end(), 0)); + + // Generate Image Warp if possible + if (input_camera_model_.try_get() && reference_frame_.try_get()) { + input_camera_info_ = {input_camera_model_.try_get().value()->getCameraIntrinsics(), + reference_frame_.try_get().value()->getCameraExtrinsics(), + input_camera_model_.try_get().value()->getDistortionModel()}; + + output_camera_intrinsics_ = output_camera_model_.try_get() + ? output_camera_model_.try_get().value()->getCameraIntrinsics() + : input_camera_info_.intrinsic; + + auto err_code = stream_->getStream()->GenerateWarpFromCameraModel( + image_warp_, image_grid_, + input_camera_info_, output_camera_intrinsics_); + if (err_code != cvcore::tensor_ops::make_error_condition( + cvcore::tensor_ops::ErrorCode::SUCCESS)) { + GXF_LOG_ERROR("image warp creation failed."); + return GXF_FAILURE; + } + } + + return GXF_SUCCESS; +} + +gxf_result_t UndistortBase::stop() { + auto err_code = stream_->getStream()->DestroyWarp(image_warp_); + if (err_code != cvcore::tensor_ops::make_error_condition( + cvcore::tensor_ops::ErrorCode::SUCCESS)) { + GXF_LOG_ERROR("image warp de-allocation failed."); + return GXF_FAILURE; + } + return GXF_SUCCESS; +} + +gxf_result_t UndistortBase::registerInterface(gxf::Registrar* registrar) { + gxf::Expected result; + + result &= registrar->parameter( + input_camera_model_, "input_camera_model", "", "", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + result &= registrar->parameter( + reference_frame_, "reference_frame", "", + "", gxf::Registrar::NoDefaultParameter(), + GXF_PARAMETER_FLAGS_OPTIONAL); + result &= registrar->parameter( + output_camera_model_, "output_camera_model", "", "", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + result &= registrar->parameter( + regions_width_, "regions_width"); + result &= registrar->parameter( + regions_height_, "regions_height"); + result &= registrar->parameter( + horizontal_intervals_, "horizontal_intervals"); + result &= registrar->parameter( + vertical_intervals_, "vertical_intervals"); + result &= registrar->parameter( + interp_type_, "interp_type"); + result &= registrar->parameter( + border_type_, "border_type"); + result &= registrar->parameter( + receiver_, "receiver"); + result &= registrar->parameter( + transmitter_, "transmitter"); + result &= registrar->parameter( + pool_, "pool"); + result &= registrar->parameter( + stream_, "stream"); + result &= registrar->parameter( + stream_pool_, "stream_pool", + "cuda stream pool", "cuda stream pool object", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + result &= registrar->parameter( + input_adapter_, "input_adapter"); + result &= registrar->parameter( + output_adapter_, "output_adapter"); + result &= registrar->parameter( + input_name_, "input_name", + "input name", "input tensor name", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + result &= registrar->parameter( + output_name_, "output_name", + "output name", "output tensor name", + gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); + + return gxf::ToResultCode(result); +} + +gxf::Expected UndistortBase::doInferOutputInfo(gxf::Entity& input) { + // Check if the input distortion type is Perpective + auto maybe_camera_message = input.get("intrinsics"); + if (maybe_camera_message) { + no_op_ = maybe_camera_message.value()->distortion_type == gxf::DistortionType::Perspective; + } + // Output size may vary, but the format must be the same + return ImageInfo{input_info_.type, static_cast(output_shape_.x), + static_cast(output_shape_.y), input_info_.is_cpu}; +} + +gxf_result_t UndistortBase::doForwardTargetCamera(gxf::Expected input_message, + gxf::Expected output_message) { + // Undistort module does not forward target_camera. Instead, it fills output intrinsics with + // target_camera intrinsics (see doUpdateCameraMessage and output_camera_intrinsics_) + return GXF_SUCCESS; +} + +gxf_result_t UndistortBase::doUpdateCameraMessage(gxf::Handle& output, + gxf::Handle& input) { + *output = *input; + (*output).distortion_type = gxf::DistortionType::Perspective; + for (size_t i = 0; i < gxf::CameraModel::kMaxDistortionCoefficients; i++) { + (*output).distortion_coefficients[i] = 0.; + } + (*output).dimensions = output_shape_; + (*output).focal_length.x = output_camera_intrinsics_.fx(); + (*output).focal_length.y = output_camera_intrinsics_.fy(); + (*output).principal_point.x = output_camera_intrinsics_.cx(); + (*output).principal_point.y = output_camera_intrinsics_.cy(); + (*output).skew_value = output_camera_intrinsics_.skew(); + return GXF_SUCCESS; +} + +#define DEFINE_UNDISTORT(INPUT_TYPE) \ + if (input_info_.type == INPUT_TYPE) { \ + return detail::UndistortImpl(output, input, output_info_, \ + input_info_, output_name, input_name, \ + stream_.get(), output_adapter_.get(), \ + input_adapter_.get(), pool_.get(), \ + image_warp_, interp.value(), border.value()); \ + } + +gxf_result_t UndistortBase::doExecute(gxf::Entity& output, gxf::Entity& input, cudaStream_t stream, + const char* output_name, const char* input_name) { + auto maybe_source_camera_message = input.get("intrinsics"); + auto maybe_target_camera_message = input.get("target_camera"); + auto target_extrinsics_delta = input.get("target_extrinsics_delta"); + if (!maybe_source_camera_message) { + if (image_warp_ == nullptr) { + GXF_LOG_ERROR("no camera information found."); + return GXF_FAILURE; + } + } else { + auto maybe_source_intrinsics = detail::GetIntrinsicsFromMessage( + maybe_source_camera_message.value()); + auto maybe_source_distortions = detail::GetDistortionsFromMessage( + maybe_source_camera_message.value()); + + if (!maybe_source_intrinsics || !maybe_source_distortions) { + return GXF_FAILURE; + } + const auto& new_source_intrinsics = maybe_source_intrinsics.value(); + const auto& new_source_distortions = maybe_source_distortions.value(); + if (!target_extrinsics_delta) { + GXF_LOG_ERROR("Target extrinsics delta is missing from input message"); + return GXF_FAILURE; + } + + if (!maybe_target_camera_message) { + GXF_LOG_ERROR("Target camera message is missing from input message"); + return GXF_FAILURE; + } + + auto new_extrinsics = detail::GetExtrinsicsFromMessage(target_extrinsics_delta.value()).value(); + + cvcore::tensor_ops::CameraIntrinsics target_camera_intrinsics; + auto maybe_target_camera_intrinsics = detail::GetIntrinsicsFromMessage( + maybe_target_camera_message.value()); + if (!maybe_target_camera_intrinsics) { + return GXF_FAILURE; + } + target_camera_intrinsics = maybe_target_camera_intrinsics.value(); + + const bool reset = image_warp_ == nullptr || + new_source_intrinsics != input_camera_info_.intrinsic || + new_source_distortions != input_camera_info_.distortion || + new_extrinsics != input_camera_info_.extrinsic || + target_camera_intrinsics != output_camera_intrinsics_; + + if (reset) { + input_camera_info_ = {new_source_intrinsics, new_extrinsics, new_source_distortions}; + output_camera_intrinsics_ = target_camera_intrinsics; + auto err_code = stream_->getStream()->GenerateWarpFromCameraModel( + image_warp_, image_grid_, input_camera_info_, output_camera_intrinsics_); + if (err_code != cvcore::tensor_ops::make_error_condition( + cvcore::tensor_ops::ErrorCode::SUCCESS)) { + GXF_LOG_ERROR("image warp creation failed."); + return GXF_FAILURE; + } + } + } + + auto interp = GetInterpolationType(interp_type_); + if (!interp) { + return interp.error(); + } + auto border = GetBorderType(border_type_); + if (!border) { + return border.error(); + } + + // Run the image undistortion operation + DEFINE_UNDISTORT(cvcore::tensor_ops::ImageType::BGR_U8); + DEFINE_UNDISTORT(cvcore::tensor_ops::ImageType::RGB_U8); + DEFINE_UNDISTORT(cvcore::tensor_ops::ImageType::NV12); + DEFINE_UNDISTORT(cvcore::tensor_ops::ImageType::NV24); + + // Return error code for unsupported type + GXF_LOG_ERROR("invalid input/output type for image undistort."); + return GXF_FAILURE; +} + +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/Undistort.hpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Undistort.hpp similarity index 65% rename from isaac_ros_image_proc/gxf/tensorops/Undistort.hpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Undistort.hpp index 377f621..2ef7165 100644 --- a/isaac_ros_image_proc/gxf/tensorops/Undistort.hpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/Undistort.hpp @@ -14,33 +14,37 @@ // limitations under the License. // // SPDX-License-Identifier: Apache-2.0 -#ifndef NVIDIA_CVCORE_UNDISTORT_HPP -#define NVIDIA_CVCORE_UNDISTORT_HPP +#pragma once -#include "CameraModel.hpp" -#include "Frame3D.hpp" -#include "TensorOperator.hpp" +#include +#include +#include "extensions/tensorops/components/CameraModel.hpp" +#include "extensions/tensorops/components/Frame3D.hpp" +#include "extensions/tensorops/components/TensorOperator.hpp" namespace nvidia { -namespace cvcore { +namespace isaac { namespace tensor_ops { // Undistort operator. class UndistortBase : public TensorOperator { -public: + public: virtual ~UndistortBase() {} - gxf_result_t start() override final; - gxf_result_t stop() override final; + gxf_result_t start() override; + gxf_result_t stop() override; gxf_result_t registerInterface(gxf::Registrar* registrar) override; -private: - gxf::Expected doInferOutputInfo(gxf::Entity& input) override final; + private: + gxf::Expected doInferOutputInfo(gxf::Entity& input) override; gxf_result_t doUpdateCameraMessage(gxf::Handle& output, - gxf::Handle& input) override final; - gxf_result_t doExecute(gxf::Entity& output, gxf::Entity& input, cudaStream_t stream, const char* output_name, - const char* input_name) override final; + gxf::Handle& input) override; + gxf_result_t doForwardTargetCamera(gxf::Expected input_message, + gxf::Expected output_message) override; + gxf_result_t doExecute(gxf::Entity& output, gxf::Entity& input, + cudaStream_t stream, const char* output_name, + const char* input_name) override; gxf::Parameter> input_camera_model_; gxf::Parameter> reference_frame_; @@ -52,18 +56,16 @@ class UndistortBase : public TensorOperator { gxf::Parameter interp_type_; gxf::Parameter border_type_; - ::cvcore::tensor_ops::ImageGrid image_grid_; - ::cvcore::tensor_ops::ImageWarp image_warp_; + cvcore::tensor_ops::ImageGrid image_grid_; + cvcore::tensor_ops::ImageWarp image_warp_; gxf::Vector2u output_shape_; - ::cvcore::CameraModel input_camera_info_; - ::cvcore::CameraIntrinsics output_camera_intrinsics_; + cvcore::tensor_ops::CameraModel input_camera_info_; + cvcore::tensor_ops::CameraIntrinsics output_camera_intrinsics_; }; class StreamUndistort : public UndistortBase {}; -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia - -#endif +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/detail/ImageAdapterTensorImpl.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/detail/ImageAdapterTensorImpl.cpp similarity index 56% rename from isaac_ros_image_proc/gxf/tensorops/detail/ImageAdapterTensorImpl.cpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/detail/ImageAdapterTensorImpl.cpp index 37099fc..7d097da 100644 --- a/isaac_ros_image_proc/gxf/tensorops/detail/ImageAdapterTensorImpl.cpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/detail/ImageAdapterTensorImpl.cpp @@ -15,31 +15,32 @@ // // SPDX-License-Identifier: Apache-2.0 #include "ImageAdapterTensorImpl.hpp" +#include namespace nvidia { -namespace cvcore { +namespace isaac { namespace tensor_ops { namespace detail { - -gxf::Expected> GetHWCIndices(const ::cvcore::ImageType type) { +using ImageType = cvcore::tensor_ops::ImageType; +gxf::Expected> GetHWCIndices(const ImageType type) { switch (type) { - case ::cvcore::ImageType::Y_U8: - case ::cvcore::ImageType::Y_U16: - case ::cvcore::ImageType::Y_F32: - case ::cvcore::ImageType::RGB_U8: - case ::cvcore::ImageType::BGR_U8: - case ::cvcore::ImageType::RGB_U16: - case ::cvcore::ImageType::BGR_U16: - case ::cvcore::ImageType::RGB_F32: - case ::cvcore::ImageType::BGR_F32: { + case ImageType::Y_U8: + case ImageType::Y_U16: + case ImageType::Y_F32: + case ImageType::RGB_U8: + case ImageType::BGR_U8: + case ImageType::RGB_U16: + case ImageType::BGR_U16: + case ImageType::RGB_F32: + case ImageType::BGR_F32: { return std::make_tuple(0, 1, 2); } - case ::cvcore::ImageType::PLANAR_RGB_U8: - case ::cvcore::ImageType::PLANAR_BGR_U8: - case ::cvcore::ImageType::PLANAR_RGB_U16: - case ::cvcore::ImageType::PLANAR_BGR_U16: - case ::cvcore::ImageType::PLANAR_RGB_F32: - case ::cvcore::ImageType::PLANAR_BGR_F32: { + case ImageType::PLANAR_RGB_U8: + case ImageType::PLANAR_BGR_U8: + case ImageType::PLANAR_RGB_U16: + case ImageType::PLANAR_BGR_U16: + case ImageType::PLANAR_RGB_F32: + case ImageType::PLANAR_BGR_F32: { return std::make_tuple(1, 2, 0); } default: { @@ -49,27 +50,28 @@ gxf::Expected> GetHWCIndices(const ::cvcore:: } } -gxf::Expected GetPrimitiveType(const ::cvcore::ImageType image_type) { +gxf::Expected GetPrimitiveType( + const cvcore::tensor_ops::ImageType image_type) { switch (image_type) { - case ::cvcore::ImageType::Y_U8: - case ::cvcore::ImageType::RGB_U8: - case ::cvcore::ImageType::BGR_U8: - case ::cvcore::ImageType::PLANAR_RGB_U8: - case ::cvcore::ImageType::PLANAR_BGR_U8: { + case cvcore::tensor_ops::ImageType::Y_U8: + case cvcore::tensor_ops::ImageType::RGB_U8: + case cvcore::tensor_ops::ImageType::BGR_U8: + case cvcore::tensor_ops::ImageType::PLANAR_RGB_U8: + case cvcore::tensor_ops::ImageType::PLANAR_BGR_U8: { return gxf::PrimitiveType::kUnsigned8; } - case ::cvcore::ImageType::Y_U16: - case ::cvcore::ImageType::RGB_U16: - case ::cvcore::ImageType::BGR_U16: - case ::cvcore::ImageType::PLANAR_RGB_U16: - case ::cvcore::ImageType::PLANAR_BGR_U16: { + case cvcore::tensor_ops::ImageType::Y_U16: + case cvcore::tensor_ops::ImageType::RGB_U16: + case cvcore::tensor_ops::ImageType::BGR_U16: + case cvcore::tensor_ops::ImageType::PLANAR_RGB_U16: + case cvcore::tensor_ops::ImageType::PLANAR_BGR_U16: { return gxf::PrimitiveType::kUnsigned16; } - case ::cvcore::ImageType::Y_F32: - case ::cvcore::ImageType::RGB_F32: - case ::cvcore::ImageType::BGR_F32: - case ::cvcore::ImageType::PLANAR_RGB_F32: - case ::cvcore::ImageType::PLANAR_BGR_F32: { + case cvcore::tensor_ops::ImageType::Y_F32: + case cvcore::tensor_ops::ImageType::RGB_F32: + case cvcore::tensor_ops::ImageType::BGR_F32: + case cvcore::tensor_ops::ImageType::PLANAR_RGB_F32: + case cvcore::tensor_ops::ImageType::PLANAR_BGR_F32: { return gxf::PrimitiveType::kFloat32; } default: { @@ -79,7 +81,8 @@ gxf::Expected GetPrimitiveType(const ::cvcore::ImageType ima } } -gxf::Expected GetTensorInfo(gxf::Handle tensor, const ::cvcore::ImageType type) { +gxf::Expected GetTensorInfo(gxf::Handle tensor, + const cvcore::tensor_ops::ImageType type) { const auto& shape = tensor->shape(); const auto rank = tensor->rank(); const auto storage_type = tensor->storage_type(); @@ -99,7 +102,7 @@ gxf::Expected GetTensorInfo(gxf::Handle tensor, const :: return ImageInfo{type, width, height, storage_type != gxf::MemoryStorageType::kDevice}; } -} // namespace detail -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia +} // namespace detail +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/detail/ImageAdapterTensorImpl.hpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/detail/ImageAdapterTensorImpl.hpp similarity index 50% rename from isaac_ros_image_proc/gxf/tensorops/detail/ImageAdapterTensorImpl.hpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/detail/ImageAdapterTensorImpl.hpp index 45a060a..d61d02c 100644 --- a/isaac_ros_image_proc/gxf/tensorops/detail/ImageAdapterTensorImpl.hpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/detail/ImageAdapterTensorImpl.hpp @@ -14,55 +14,62 @@ // limitations under the License. // // SPDX-License-Identifier: Apache-2.0 -#ifndef NVIDIA_CVCORE_TENSOR_ADAPTER_HPP -#define NVIDIA_CVCORE_TENSOR_ADAPTER_HPP - -#include "../ImageUtils.hpp" +#pragma once #include -#include "cv/core/Image.h" +#include "extensions/tensorops/components/ImageUtils.hpp" +#include "extensions/tensorops/core/Image.h" #include "gxf/std/allocator.hpp" #include "gxf/std/tensor.hpp" namespace nvidia { -namespace cvcore { +namespace isaac { namespace tensor_ops { namespace detail { -gxf::Expected> GetHWCIndices(const ::cvcore::ImageType type); +gxf::Expected> GetHWCIndices( + const cvcore::tensor_ops::ImageType type); -gxf::Expected GetPrimitiveType(const ::cvcore::ImageType image_type); +gxf::Expected GetPrimitiveType( + const cvcore::tensor_ops::ImageType image_type); -gxf::Expected GetTensorInfo(gxf::Handle tensor, const ::cvcore::ImageType type); +gxf::Expected GetTensorInfo(gxf::Handle tensor, + const cvcore::tensor_ops::ImageType type); -template<::cvcore::ImageType T, - typename std::enable_if::type* = nullptr> -gxf::Expected<::cvcore::Image> WrapImageFromTensor(gxf::Handle tensor) { +template::type* = nullptr> +gxf::Expected> WrapImageFromTensor( + gxf::Handle tensor) { const auto info = GetTensorInfo(tensor, T); if (!info) { return gxf::Unexpected{GXF_FAILURE}; } - using D = typename ::cvcore::detail::ChannelTypeToNative<::cvcore::ImageTraits::CT>::Type; + using D = typename cvcore::tensor_ops::detail:: + ChannelTypeToNative::CT>::Type; auto pointer = tensor->data(); if (!pointer) { return gxf::Unexpected{GXF_FAILURE}; } const size_t stride = tensor->stride(std::get<0>(GetHWCIndices(T).value())); - return ::cvcore::Image(info.value().width, info.value().height, stride, pointer.value(), info.value().is_cpu); + return cvcore::tensor_ops::Image(info.value().width, info.value().height, + stride, pointer.value(), info.value().is_cpu); } -template<::cvcore::ImageType T, - typename std::enable_if::type* = nullptr> -gxf::Expected<::cvcore::Image> WrapImageFromTensor(gxf::Handle tensor) { +template::type* = nullptr> +gxf::Expected> WrapImageFromTensor(gxf::Handle tensor) { GXF_LOG_ERROR("NV12/NV24 not supported for gxf::Tensor"); return gxf::Unexpected{GXF_FAILURE}; } -template<::cvcore::ImageType T, - typename std::enable_if::type* = nullptr> +template::type* = nullptr> gxf_result_t AllocateTensor(gxf::Handle tensor, size_t width, size_t height, - gxf::Handle allocator, bool is_cpu, bool allocate_pitch_linear) { + gxf::Handle allocator, bool is_cpu, bool allocate_pitch_linear) { const auto primitive_type = GetPrimitiveType(T); if (!primitive_type) { return primitive_type.error(); @@ -75,13 +82,15 @@ gxf_result_t AllocateTensor(gxf::Handle tensor, size_t width, size_ std::array dims; dims[std::get<0>(indices.value())] = height; dims[std::get<1>(indices.value())] = width; - dims[std::get<2>(indices.value())] = ::cvcore::detail::ChannelToCount<::cvcore::ImageTraits::CC>(); + dims[std::get<2>(indices.value())] = + cvcore::tensor_ops::detail::ChannelToCount::CC>(); const gxf::Shape shape(dims, 3); auto result = - tensor->reshapeCustom(shape, primitive_type.value(), gxf::PrimitiveTypeSize(primitive_type.value()), - gxf::Unexpected{GXF_UNINITIALIZED_VALUE}, - is_cpu ? gxf::MemoryStorageType::kHost : gxf::MemoryStorageType::kDevice, allocator); + tensor->reshapeCustom(shape, primitive_type.value(), + gxf::PrimitiveTypeSize(primitive_type.value()), + gxf::Unexpected{GXF_UNINITIALIZED_VALUE}, + is_cpu ? gxf::MemoryStorageType::kHost : gxf::MemoryStorageType::kDevice, allocator); if (!result) { GXF_LOG_ERROR("reshape tensor failed."); return GXF_FAILURE; @@ -89,17 +98,16 @@ gxf_result_t AllocateTensor(gxf::Handle tensor, size_t width, size_ return GXF_SUCCESS; } -template<::cvcore::ImageType T, - typename std::enable_if::type* = nullptr> +template::type* = nullptr> gxf_result_t AllocateTensor(gxf::Handle tensor, size_t width, size_t height, - gxf::Handle allocator, bool is_cpu, bool allocate_pitch_linear) { + gxf::Handle allocator, bool is_cpu, bool allocate_pitch_linear) { GXF_LOG_ERROR("NV12/NV24 not supported for gxf::Tensor"); return GXF_FAILURE; } -} // namespace detail -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia - -#endif +} // namespace detail +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/detail/ImageAdapterVideoBufferImpl.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/detail/ImageAdapterVideoBufferImpl.cpp similarity index 73% rename from isaac_ros_image_proc/gxf/tensorops/detail/ImageAdapterVideoBufferImpl.cpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/detail/ImageAdapterVideoBufferImpl.cpp index ac257b4..2e77d81 100644 --- a/isaac_ros_image_proc/gxf/tensorops/detail/ImageAdapterVideoBufferImpl.cpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/detail/ImageAdapterVideoBufferImpl.cpp @@ -17,52 +17,53 @@ #include "ImageAdapterVideoBufferImpl.hpp" namespace nvidia { -namespace cvcore { +namespace isaac { namespace tensor_ops { namespace detail { - -gxf::Expected<::cvcore::ImageType> GetImageTypeFromVideoFormat(const gxf::VideoFormat format) { +using ImageType = cvcore::tensor_ops::ImageType; +gxf::Expected GetImageTypeFromVideoFormat( + const gxf::VideoFormat format) { switch (format) { case gxf::VideoFormat::GXF_VIDEO_FORMAT_NV12: case gxf::VideoFormat::GXF_VIDEO_FORMAT_NV12_ER: { - return ::cvcore::ImageType::NV12; + return ImageType::NV12; } case gxf::VideoFormat::GXF_VIDEO_FORMAT_NV24: case gxf::VideoFormat::GXF_VIDEO_FORMAT_NV24_ER: { - return ::cvcore::ImageType::NV24; + return ImageType::NV24; } case gxf::VideoFormat::GXF_VIDEO_FORMAT_RGBA: { - return ::cvcore::ImageType::RGBA_U8; + return ImageType::RGBA_U8; } case gxf::VideoFormat::GXF_VIDEO_FORMAT_RGB: { - return ::cvcore::ImageType::RGB_U8; + return ImageType::RGB_U8; } case gxf::VideoFormat::GXF_VIDEO_FORMAT_RGB32: { - return ::cvcore::ImageType::RGB_F32; + return ImageType::RGB_F32; } case gxf::VideoFormat::GXF_VIDEO_FORMAT_BGR: { - return ::cvcore::ImageType::BGR_U8; + return ImageType::BGR_U8; } case gxf::VideoFormat::GXF_VIDEO_FORMAT_BGR32: { - return ::cvcore::ImageType::BGR_F32; + return ImageType::BGR_F32; } case gxf::VideoFormat::GXF_VIDEO_FORMAT_R8_G8_B8: { - return ::cvcore::ImageType::PLANAR_RGB_U8; + return ImageType::PLANAR_RGB_U8; } case gxf::VideoFormat::GXF_VIDEO_FORMAT_R32_G32_B32: { - return ::cvcore::ImageType::PLANAR_RGB_F32; + return ImageType::PLANAR_RGB_F32; } case gxf::VideoFormat::GXF_VIDEO_FORMAT_B8_G8_R8: { - return ::cvcore::ImageType::PLANAR_BGR_U8; + return ImageType::PLANAR_BGR_U8; } case gxf::VideoFormat::GXF_VIDEO_FORMAT_B32_G32_R32: { - return ::cvcore::ImageType::PLANAR_BGR_F32; + return ImageType::PLANAR_BGR_F32; } case gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY: { - return ::cvcore::ImageType::Y_U8; + return ImageType::Y_U8; } - case gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY32: { - return ::cvcore::ImageType::Y_F32; + case gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY32F: { + return ImageType::Y_F32; } default: { GXF_LOG_ERROR("invalid video format."); @@ -82,7 +83,7 @@ gxf::Expected GetVideoBufferInfo(gxf::Handle video_ storage_type != gxf::MemoryStorageType::kDevice}; } -} // namespace detail -} // namespace tensor_ops -} // namespace cvcore -} // namespace nvidia +} // namespace detail +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/detail/ImageAdapterVideoBufferImpl.hpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/detail/ImageAdapterVideoBufferImpl.hpp new file mode 100644 index 0000000..d19d88a --- /dev/null +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/components/detail/ImageAdapterVideoBufferImpl.hpp @@ -0,0 +1,210 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 +#pragma once + +#include +#include "extensions/tensorops/components/ImageUtils.hpp" +#include "gxf/multimedia/video.hpp" +#include "gxf/std/allocator.hpp" + +#include "extensions/tensorops/core/Image.h" +#include "gems/video_buffer/allocator.hpp" +namespace nvidia { +namespace isaac { +namespace tensor_ops { +namespace detail { + +gxf::Expected GetImageTypeFromVideoFormat( + const gxf::VideoFormat format); + +gxf::Expected GetVideoBufferInfo(gxf::Handle video_buffer); + +template::type* = nullptr> +gxf::Expected> WrapImageFromVideoBuffer( + gxf::Handle video_buffer) { + const auto info = GetVideoBufferInfo(video_buffer); + if (!info) { + return gxf::Unexpected{GXF_FAILURE}; + } + using D = typename cvcore::tensor_ops::detail:: + ChannelTypeToNative::CT>::Type; + auto pointer = reinterpret_cast(video_buffer->pointer()); + if (!pointer) { + return gxf::Unexpected{GXF_FAILURE}; + } + const auto& color_planes = video_buffer->video_frame_info().color_planes; + return cvcore::tensor_ops::Image(info.value().width, info.value().height, + color_planes[0].stride, pointer, + info.value().is_cpu); +} + +template::type* = nullptr> +gxf::Expected> WrapImageFromVideoBuffer( + gxf::Handle video_buffer) { + const auto info = GetVideoBufferInfo(video_buffer); + if (!info) { + return gxf::Unexpected{GXF_FAILURE}; + } + // Note only U8 is supported in NV12/NV24 + auto pointer = reinterpret_cast(video_buffer->pointer()); + if (!pointer) { + return gxf::Unexpected{GXF_FAILURE}; + } + const auto& color_planes = video_buffer->video_frame_info().color_planes; + return cvcore::tensor_ops::Image(info.value().width, info.value().height, + color_planes[0].stride, color_planes[1].stride, + pointer, pointer + color_planes[1].offset, info.value().is_cpu); +} + +template +struct ImageTypeToVideoFormat { + static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_CUSTOM; +}; + +template<> +struct ImageTypeToVideoFormat { + static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_NV12_ER; +}; + +template<> +struct ImageTypeToVideoFormat { + static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_NV24_ER; +}; + +template<> +struct ImageTypeToVideoFormat { + static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_RGBA; +}; + +template<> +struct ImageTypeToVideoFormat { + static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_RGB; +}; + +template<> +struct ImageTypeToVideoFormat { + static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_RGB32; +}; + +template<> +struct ImageTypeToVideoFormat { + static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_BGR; +}; + +template<> +struct ImageTypeToVideoFormat { + static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_BGR32; +}; + +template<> +struct ImageTypeToVideoFormat { + static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_R8_G8_B8; +}; + +template<> +struct ImageTypeToVideoFormat { + static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_R32_G32_B32; +}; + +template<> +struct ImageTypeToVideoFormat { + static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_B8_G8_R8; +}; + +template<> +struct ImageTypeToVideoFormat { + static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_B32_G32_R32; +}; + +template<> +struct ImageTypeToVideoFormat { + static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY; +}; + +template<> +struct ImageTypeToVideoFormat { + static constexpr gxf::VideoFormat format = gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY32F; +}; + +// This include the list of image types that GXF supported so far +constexpr bool IsValidGXFImageType(const cvcore::tensor_ops::ImageType type) { + return type == cvcore::tensor_ops::ImageType::NV12 || + type == cvcore::tensor_ops::ImageType::NV24 || + type == cvcore::tensor_ops::ImageType::RGBA_U8 || + type == cvcore::tensor_ops::ImageType::RGB_U8 || + type == cvcore::tensor_ops::ImageType::BGR_U8 || + type == cvcore::tensor_ops::ImageType::RGB_F32 || + type == cvcore::tensor_ops::ImageType::BGR_F32 || + type == cvcore::tensor_ops::ImageType::PLANAR_RGB_U8 || + type == cvcore::tensor_ops::ImageType::PLANAR_BGR_U8 || + type == cvcore::tensor_ops::ImageType::PLANAR_RGB_F32 || + type == cvcore::tensor_ops::ImageType::PLANAR_BGR_F32 || + type == cvcore::tensor_ops::ImageType::Y_U8 || + type == cvcore::tensor_ops::ImageType::Y_F32; +} + +template::type* = nullptr> +gxf_result_t AllocateVideoBuffer(gxf::Handle video_buffer, + size_t width, size_t height, + gxf::Handle allocator, + bool is_cpu, bool allocate_pitch_linear) { + if (width % 2 != 0 || height % 2 != 0) { + GXF_LOG_ERROR("image width/height must be even for creation of gxf::VideoBuffer"); + return GXF_FAILURE; + } + if (allocate_pitch_linear) { + auto result = video_buffer->resize::format>( + static_cast(width), static_cast(height), + gxf::SurfaceLayout::GXF_SURFACE_LAYOUT_PITCH_LINEAR, + is_cpu ? gxf::MemoryStorageType::kHost : gxf::MemoryStorageType::kDevice, allocator); + + if (!result) { + GXF_LOG_ERROR("resize VideoBuffer failed."); + return GXF_FAILURE; + } + } else { + auto result = AllocateUnpaddedVideoBuffer::format>( + video_buffer, static_cast(width), static_cast(height), + is_cpu ? gxf::MemoryStorageType::kHost : gxf::MemoryStorageType::kDevice, + allocator); + if (!result) { + GXF_LOG_ERROR("custom resize VideoBuffer failed."); + return GXF_FAILURE; + } + } + return GXF_SUCCESS; +} + +template::type* = nullptr> +gxf_result_t AllocateVideoBuffer(gxf::Handle video_buffer, + size_t width, size_t height, + gxf::Handle allocator, + bool is_cpu, bool allocate_pitch_linear) { + GXF_LOG_ERROR("image type not supported in gxf::VideoBuffer"); + return GXF_FAILURE; +} + +} // namespace detail +} // namespace tensor_ops +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/ArithmeticOperations.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/ArithmeticOperations.cpp new file mode 100644 index 0000000..de0ff46 --- /dev/null +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/ArithmeticOperations.cpp @@ -0,0 +1,361 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2020-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include + +#include "extensions/tensorops/core/ImageUtils.h" +#include "extensions/tensorops/core/Memory.h" +#include "extensions/tensorops/core/NppUtils.h" +#include "nppi_arithmetic_and_logical_operations.h" +#include "nppi_data_exchange_and_initialization.h" + +namespace cvcore { +namespace tensor_ops { + +namespace { + +static bool NormalizeTensorC3F32Inplace(Tensor& src, const float scale[3], + const float offset[3], NppStreamContext streamContext) { + const int srcW = src.getWidth(); + const int srcH = src.getHeight(); + const NppiSize srcSize = {srcW, srcH}; + + const Npp32f offsets[3] = {static_cast(offset[0]), static_cast(offset[1]), + static_cast(offset[2])}; + NppStatus status = + nppiAddC_32f_C3IR_Ctx(offsets, static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), srcSize, streamContext); + if (status != NPP_SUCCESS) { + return false; + } + + const Npp32f scales[3] = {static_cast(scale[0]), static_cast(scale[1]), + static_cast(scale[2])}; + status = nppiMulC_32f_C3IR_Ctx(scales, static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), srcSize, streamContext); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +template +static bool NormalizeTensorC1F32Inplace(Tensor& src, const float scale, + const float offset, NppStreamContext streamContext) { + const int srcW = src.getWidth(); + const int srcH = src.getHeight(); + const NppiSize srcSize = {srcW, srcH}; + + NppStatus status = + nppiAddC_32f_C1IR_Ctx(static_cast(offset), static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), srcSize, streamContext); + if (status != NPP_SUCCESS) { + return false; + } + + status = nppiMulC_32f_C1IR_Ctx(static_cast(scale), static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), srcSize, streamContext); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +template +bool NormalizeC1U8Impl(Tensor& dst, const Tensor& src, + const float scale, const float offset, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if ((src.getWidth() != dst.getWidth()) || (src.getHeight() != dst.getHeight())) { + return false; + } + + NppStreamContext streamContext = GetNppStreamContext(stream); + + NppStatus status = nppiConvert_8u32f_C1R_Ctx( + static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + {static_cast(src.getWidth()), static_cast(src.getHeight())}, streamContext); + if (status != NPP_SUCCESS) { + return false; + } + + return NormalizeTensorC1F32Inplace(dst, scale, offset, streamContext); +} + +template +bool NormalizeC1U16Impl(Tensor& dst, const Tensor& src, + const float scale, const float offset, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if ((src.getWidth() != dst.getWidth()) || (src.getHeight() != dst.getHeight())) { + return false; + } + NppStreamContext streamContext = GetNppStreamContext(stream); + + NppStatus status = nppiConvert_16u32f_C1R_Ctx( + static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + {static_cast(src.getWidth()), static_cast(src.getHeight())}, streamContext); + if (status != NPP_SUCCESS) { + return false; + } + + return NormalizeTensorC1F32Inplace(dst, scale, offset, streamContext); +} + +template +bool NormalizeC1F32Impl(Tensor& dst, const Tensor& src, + const float scale, const float offset, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if ((src.getWidth() != dst.getWidth()) || (src.getHeight() != dst.getHeight())) { + return false; + } + + Copy(dst, src, stream); + return NormalizeTensorC1F32Inplace(dst, scale, offset, GetNppStreamContext(stream)); +} + +template +bool NormalizeC3Batch(Tensor& dst, Tensor& src, const float scale[3], + const float offset[3], cudaStream_t stream) { + // src and dst must be GPU tensors + if ((src.isCPU() || dst.isCPU()) || (src.getDepth() != dst.getDepth())) { + return false; + } + + for (size_t i = 0; i < src.getDepth(); i++) { + size_t shiftSrc = i * src.getStride(TensorDimension::DEPTH); + size_t shiftDst = i * dst.getStride(TensorDimension::DEPTH); + Tensor srcTmp(src.getWidth(), src.getHeight(), + src.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), + src.getData() + shiftSrc, false); + Tensor dstTmp(dst.getWidth(), dst.getHeight(), + dst.getStride(TensorDimension::HEIGHT) * GetChannelSize(F32), + dst.getData() + shiftDst, false); + bool ret = Normalize(dstTmp, srcTmp, scale, offset, stream); + if (!ret) { + return false; + } + } + return true; +} + +template +bool NormalizeC1Batch(Tensor& dst, Tensor& src, + const float scale, const float offset, cudaStream_t stream) { + // src and dst must be GPU tensors + if ((src.isCPU() || dst.isCPU()) || (src.getDepth() != dst.getDepth())) { + return false; + } + + for (size_t i = 0; i < src.getDepth(); i++) { + size_t shiftSrc = i * src.getStride(TensorDimension::DEPTH); + size_t shiftDst = i * dst.getStride(TensorDimension::DEPTH); + Tensor srcTmp(src.getWidth(), src.getHeight(), + src.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), + src.getData() + shiftSrc, false); + Tensor dstTmp(dst.getWidth(), dst.getHeight(), + dst.getStride(TensorDimension::HEIGHT) * GetChannelSize(F32), + dst.getData() + shiftDst, false); + bool ret = Normalize(dstTmp, srcTmp, scale, offset, stream); + if (!ret) { + return false; + } + } + return true; +} + +template +bool NormalizeC1Batch(Tensor& dst, Tensor& src, + const float scale, const float offset, cudaStream_t stream) { + // src and dst must be GPU tensors + if ((src.isCPU() || dst.isCPU()) || (src.getDepth() != dst.getDepth())) { + return false; + } + + for (size_t i = 0; i < src.getDepth(); i++) { + size_t shiftSrc = i * src.getStride(TensorDimension::DEPTH); + size_t shiftDst = i * dst.getStride(TensorDimension::DEPTH); + Tensor srcTmp(src.getWidth(), src.getHeight(), + src.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), + src.getData() + shiftSrc, false); + Tensor dstTmp(dst.getWidth(), dst.getHeight(), + dst.getStride(TensorDimension::HEIGHT) * GetChannelSize(F32), + dst.getData() + shiftDst, false); + bool ret = Normalize(dstTmp, srcTmp, scale, offset, stream); + if (!ret) { + return false; + } + } + return true; +} + +} // anonymous namespace + +bool Normalize(Tensor& dst, const Tensor& src, + const float scale[3], const float offset[3], cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if ((src.getWidth() != dst.getWidth()) || (src.getHeight() != dst.getHeight())) { + return false; + } + + NppStreamContext streamContext = GetNppStreamContext(stream); + + NppStatus status = nppiConvert_8u32f_C3R_Ctx( + static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + {static_cast(src.getWidth()), static_cast(src.getHeight())}, streamContext); + if (status != NPP_SUCCESS) { + return false; + } + + return NormalizeTensorC3F32Inplace(dst, scale, offset, streamContext); +} + +bool Normalize(Tensor& dst, const Tensor& src, + const float scale[3], const float offset[3], cudaStream_t stream) { + return NormalizeC3Batch(dst, const_cast &>(src), scale, offset, stream); +} + +bool Normalize(Tensor& dst, const Tensor& src, + const float scale[3], const float offset[3], cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if ((src.getWidth() != dst.getWidth()) || (src.getHeight() != dst.getHeight())) { + return false; + } + NppStreamContext streamContext = GetNppStreamContext(stream); + + NppStatus status = nppiConvert_16u32f_C3R_Ctx( + static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + {static_cast(src.getWidth()), static_cast(src.getHeight())}, streamContext); + if (status != NPP_SUCCESS) { + return false; + } + + return NormalizeTensorC3F32Inplace(dst, scale, offset, streamContext); +} + +bool Normalize(Tensor& dst, const Tensor& src, const float scale[3], + const float offset[3], cudaStream_t stream) { + return (NormalizeC3Batch(dst, const_cast &>(src), scale, offset, stream)); +} + +bool Normalize(Tensor& dst, const Tensor& src, + const float scale[3], const float offset[3], cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if ((src.getWidth() != dst.getWidth()) || (src.getHeight() != dst.getHeight())) { + return false; + } + Copy(dst, src, stream); + return NormalizeTensorC3F32Inplace(dst, scale, offset, GetNppStreamContext(stream)); +} + +bool Normalize(Tensor & dst, const Tensor & src, const float scale[3], + const float offset[3], cudaStream_t stream) { + return (NormalizeC3Batch(dst, const_cast &>(src), scale, offset, stream)); +} + +bool Normalize(Tensor& dst, const Tensor& src, + const float scale, const float offset, cudaStream_t stream) { + return NormalizeC1U8Impl(dst, src, scale, offset, stream); +} + +bool Normalize(Tensor& dst, const Tensor& src, + const float scale, const float offset, cudaStream_t stream) { + return NormalizeC1Batch(dst, const_cast &>(src), scale, offset, stream); +} + +bool Normalize(Tensor& dst, const Tensor& src, + const float scale, const float offset, cudaStream_t stream) { + return NormalizeC1U16Impl(dst, src, scale, offset, stream); +} + +bool Normalize(Tensor& dst, const Tensor& src, + const float scale, const float offset, cudaStream_t stream) { + return NormalizeC1Batch(dst, const_cast &>(src), scale, offset, stream); +} + +bool Normalize(Tensor& dst, const Tensor& src, + const float scale, const float offset, cudaStream_t stream) { + return NormalizeC1F32Impl(dst, src, scale, offset, stream); +} + +bool Normalize(Tensor& dst, const Tensor& src, + const float scale, const float offset, cudaStream_t stream) { + return NormalizeC1Batch(dst, const_cast &>(src), scale, offset, stream); +} + +bool Normalize(Tensor& dst, const Tensor& src, + const float scale, const float offset, cudaStream_t stream) { + return NormalizeC1U8Impl(dst, src, scale, offset, stream); +} + +bool Normalize(Tensor& dst, const Tensor& src, + const float scale, const float offset, cudaStream_t stream) { + return NormalizeC1Batch(dst, const_cast &>(src), scale, offset, stream); +} + +bool Normalize(Tensor& dst, const Tensor& src, + const float scale, const float offset, cudaStream_t stream) { + return NormalizeC1U16Impl(dst, src, scale, offset, stream); +} + +bool Normalize(Tensor& dst, const Tensor& src, + const float scale, const float offset, cudaStream_t stream) { + return NormalizeC1Batch(dst, const_cast &>(src), scale, offset, stream); +} + +bool Normalize(Tensor& dst, const Tensor& src, + const float scale, const float offset, cudaStream_t stream) { + return NormalizeC1F32Impl(dst, src, scale, offset, stream); +} + +bool Normalize(Tensor& dst, const Tensor& src, + const float scale, const float offset, cudaStream_t stream) { + return NormalizeC1Batch(dst, const_cast &>(src), scale, offset, stream); +} + +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/core/cvcore/Array.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Array.cpp similarity index 71% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/src/core/cvcore/Array.cpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Array.cpp index 05cd535..3eae22a 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/core/cvcore/Array.cpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Array.cpp @@ -15,7 +15,7 @@ // // SPDX-License-Identifier: Apache-2.0 -#include "cv/core/Array.h" +#include "extensions/tensorops/core/Array.h" #include @@ -23,6 +23,7 @@ #include namespace cvcore { +namespace tensor_ops { ArrayBase::ArrayBase() : m_data{nullptr} @@ -30,13 +31,11 @@ ArrayBase::ArrayBase() , m_capacity{0} , m_elemSize{0} , m_isOwning{false} - , m_isCPU{true} -{ + , m_isCPU{true} { } -ArrayBase::ArrayBase(std::size_t capacity, std::size_t elemSize, void *dataPtr, bool isCPU) - : ArrayBase() -{ +ArrayBase::ArrayBase(std::size_t capacity, std::size_t elemSize, void* dataPtr, bool isCPU) + : ArrayBase() { m_isOwning = false; m_isCPU = isCPU; m_elemSize = elemSize; @@ -45,51 +44,38 @@ ArrayBase::ArrayBase(std::size_t capacity, std::size_t elemSize, void *dataPtr, } ArrayBase::ArrayBase(std::size_t capacity, std::size_t elemSize, bool isCPU) - : ArrayBase(capacity, elemSize, nullptr, isCPU) -{ + : ArrayBase(capacity, elemSize, nullptr, isCPU) { m_isOwning = true; // allocate const size_t arraySize = capacity * elemSize; - if (arraySize > 0) - { - if (isCPU) - { + if (arraySize > 0) { + if (isCPU) { m_data = std::malloc(arraySize); - } - else - { - if (cudaMalloc(&m_data, arraySize) != 0) - { + } else { + if (cudaMalloc(&m_data, arraySize) != 0) { throw std::runtime_error("CUDA alloc() failed!"); } } } } -ArrayBase::~ArrayBase() -{ - if (m_isOwning) - { - if (m_isCPU) - { +ArrayBase::~ArrayBase() { + if (m_isOwning) { + if (m_isCPU) { std::free(m_data); - } - else - { + } else { cudaFree(m_data); } } } ArrayBase::ArrayBase(ArrayBase &&t) - : ArrayBase() -{ + : ArrayBase() { *this = std::move(t); } -ArrayBase &ArrayBase::operator=(ArrayBase &&t) -{ +ArrayBase& ArrayBase::operator=(ArrayBase && t) { using std::swap; swap(m_data, t.m_data); @@ -102,44 +88,37 @@ ArrayBase &ArrayBase::operator=(ArrayBase &&t) return *this; } -void *ArrayBase::getElement(int idx) const -{ +void* ArrayBase::getElement(int idx) const { assert(idx >= 0 && idx < m_capacity); return reinterpret_cast(m_data) + idx * m_elemSize; } -std::size_t ArrayBase::getSize() const -{ +std::size_t ArrayBase::getSize() const { return m_size; } -std::size_t ArrayBase::getCapacity() const -{ +std::size_t ArrayBase::getCapacity() const { return m_capacity; } -std::size_t ArrayBase::getElementSize() const -{ +std::size_t ArrayBase::getElementSize() const { return m_elemSize; } -void ArrayBase::setSize(std::size_t size) -{ +void ArrayBase::setSize(std::size_t size) { assert(size <= m_capacity); m_size = size; } -bool ArrayBase::isCPU() const -{ +bool ArrayBase::isCPU() const { return m_isCPU; } -bool ArrayBase::isOwning() const -{ +bool ArrayBase::isOwning() const { return m_isOwning; } -void *ArrayBase::getData() const -{ +void* ArrayBase::getData() const { return m_data; } -} // namespace cvcore +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/Array.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Array.h similarity index 80% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/Array.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Array.h index c9f23d8..5093c48 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/Array.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Array.h @@ -15,21 +15,20 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_ARRAY_H -#define CVCORE_ARRAY_H +#pragma once #include #include #include namespace cvcore { +namespace tensor_ops { /** * Base implementation of Array. */ -class ArrayBase -{ -public: +class ArrayBase { + public: /** * Constructor of a non-owning arrays. * @param capacity capacity of the array. @@ -37,7 +36,7 @@ class ArrayBase * @param dataPtr data pointer to the raw source array. * @param isCPU whether to allocate the array on CPU or GPU. */ - ArrayBase(std::size_t capacity, std::size_t elemSize, void *dataPtr, bool isCPU); + ArrayBase(std::size_t capacity, std::size_t elemSize, void * dataPtr, bool isCPU); /** * Constructor of a memory-owning arrays @@ -55,12 +54,12 @@ class ArrayBase /** * ArrayBase is non-copyable. */ - ArrayBase(const ArrayBase &) = delete; + ArrayBase(const ArrayBase&) = delete; /** * ArrayBase is non-copyable. */ - ArrayBase &operator=(const ArrayBase &) = delete; + ArrayBase& operator=(const ArrayBase &) = delete; /** * Move constructor of ArrayBase. @@ -70,14 +69,14 @@ class ArrayBase /** * Move assignment operator of ArrayBase. */ - ArrayBase &operator=(ArrayBase &&); + ArrayBase& operator=(ArrayBase &&); /** * Get the pointer to specified index. * @param idx element index. * @return pointer to the specified element. */ - void *getElement(int idx) const; + void* getElement(int idx) const; /** * Get the size of the array. @@ -119,12 +118,12 @@ class ArrayBase * Get the raw pointer to the array data. * @return void pointer to the first element of the array. */ - void *getData() const; + void* getData() const; -private: + private: ArrayBase(); - void *m_data; + void* m_data; std::size_t m_size; std::size_t m_capacity; std::size_t m_elemSize; @@ -137,15 +136,13 @@ class ArrayBase * @tparam T type of element in array. */ template -class Array : public ArrayBase -{ -public: +class Array : public ArrayBase { + public: /** * Default constructor of an array. */ Array() - : ArrayBase{0, sizeof(T), nullptr, true} - { + : ArrayBase{0, sizeof(T), nullptr, true} { } /** @@ -155,9 +152,8 @@ class Array : public ArrayBase * @param dataPtr data pointer to the raw source array. * @param isCPU whether to allocate array on CPU or GPU. */ - Array(std::size_t size, std::size_t capacity, void *dataPtr, bool isCPU = true) - : ArrayBase{capacity, sizeof(T), dataPtr, isCPU} - { + Array(std::size_t size, std::size_t capacity, void * dataPtr, bool isCPU = true) + : ArrayBase{capacity, sizeof(T), dataPtr, isCPU} { ArrayBase::setSize(size); } @@ -167,15 +163,13 @@ class Array : public ArrayBase * @param isCPU whether to allocate array on CPU or GPU. */ Array(std::size_t capacity, bool isCPU = true) - : ArrayBase{capacity, sizeof(T), isCPU} - { + : ArrayBase{capacity, sizeof(T), isCPU} { } /** * Destructor of the Array. */ - ~Array() - { + ~Array() { // call resize here such that CPU-based destructor // will call destructors of the objects stored // in the array before deallocating the storage @@ -185,27 +179,25 @@ class Array : public ArrayBase /** * Array is non-copyable. */ - Array(const Array &) = delete; + Array(const Array&) = delete; /** * Array is non-copyable. */ - Array &operator=(const Array &) = delete; + Array& operator=(const Array &) = delete; /** * Move constructor of Array. */ Array(Array &&t) - : Array() - { + : Array() { *this = std::move(t); } /** * Move assignment operator of Array. */ - Array &operator=(Array &&t) - { + Array& operator=(Array &&t) { static_cast(*this) = std::move(t); return *this; } @@ -214,20 +206,16 @@ class Array : public ArrayBase * Set size of the Array. * @param size size of the Array. */ - void setSize(std::size_t size) - { + void setSize(std::size_t size) { const std::size_t oldSize = getSize(); ArrayBase::setSize(size); - if (isCPU()) - { + if (isCPU()) { // shrinking case - for (std::size_t i = size; i < oldSize; ++i) - { + for (std::size_t i = size; i < oldSize; ++i) { reinterpret_cast(getElement(i))->~T(); } // expanding case - for (std::size_t i = oldSize; i < size; ++i) - { + for (std::size_t i = oldSize; i < size; ++i) { new (getElement(i)) T; } } @@ -238,8 +226,7 @@ class Array : public ArrayBase * @param idx index of element. * @return const reference to the specified element. */ - const T &operator[](int idx) const - { + const T& operator[](int idx) const { assert(idx >= 0 && idx < getSize()); return *reinterpret_cast(getElement(idx)); } @@ -249,8 +236,7 @@ class Array : public ArrayBase * @param idx index of element. * @return reference to the specified element. */ - T &operator[](int idx) - { + T& operator[](int idx) { assert(idx >= 0 && idx < getSize()); return *reinterpret_cast(getElement(idx)); } @@ -262,15 +248,13 @@ class Array : public ArrayBase * @tparam N capacity of array. */ template -class ArrayN : public ArrayBase -{ -public: +class ArrayN : public ArrayBase { + public: /** * Default constructor of ArrayN (create an owning Tensor with capacity N). */ ArrayN() - : ArrayBase{N, sizeof(T), true} - { + : ArrayBase{N, sizeof(T), true} { setSize(N); } @@ -280,9 +264,8 @@ class ArrayN : public ArrayBase * @param dataPtr data pointer to the raw source array. * @param isCPU whether to allocate array on CPU or GPU. */ - ArrayN(std::size_t size, void *dataPtr, bool isCPU = true) - : ArrayBase{N, sizeof(T), dataPtr, isCPU} - { + ArrayN(std::size_t size, void * dataPtr, bool isCPU = true) + : ArrayBase{N, sizeof(T), dataPtr, isCPU} { ArrayBase::setSize(size); } @@ -291,16 +274,14 @@ class ArrayN : public ArrayBase * @param isCPU whether to allocate array on CPU or GPU. */ ArrayN(bool isCPU) - : ArrayBase{N, sizeof(T), isCPU} - { + : ArrayBase{N, sizeof(T), isCPU} { setSize(N); } /** * Destructor of the ArrayN. */ - ~ArrayN() - { + ~ArrayN() { // call resize here such that CPU-based destructor // will call destructors of the objects stored // in the array before deallocating the storage @@ -310,27 +291,25 @@ class ArrayN : public ArrayBase /** * ArrayN is non-copyable. */ - ArrayN(const ArrayN &) = delete; + ArrayN(const ArrayN&) = delete; /** * ArrayN is non-copyable. */ - ArrayN &operator=(const ArrayN &) = delete; + ArrayN& operator=(const ArrayN &) = delete; /** * Move constructor of ArrayN. */ ArrayN(ArrayN &&t) - : ArrayN() - { + : ArrayN() { *this = std::move(t); } /** * Move assignment operator of ArrayN. */ - ArrayN &operator=(ArrayN &&t) - { + ArrayN& operator=(ArrayN &&t) { static_cast(*this) = std::move(t); return *this; } @@ -339,20 +318,16 @@ class ArrayN : public ArrayBase * Set size of the ArrayN. * @param size size of the ArrayN. */ - void setSize(std::size_t size) - { + void setSize(std::size_t size) { const std::size_t oldSize = getSize(); ArrayBase::setSize(size); - if (isCPU()) - { + if (isCPU()) { // shrinking case - for (std::size_t i = size; i < oldSize; ++i) - { + for (std::size_t i = size; i < oldSize; ++i) { reinterpret_cast(getElement(i))->~T(); } // expanding case - for (std::size_t i = oldSize; i < size; ++i) - { + for (std::size_t i = oldSize; i < size; ++i) { new (getElement(i)) T; } } @@ -363,8 +338,7 @@ class ArrayN : public ArrayBase * @param idx index of element. * @return const reference to the specified element. */ - const T &operator[](int idx) const - { + const T& operator[](int idx) const { assert(idx >= 0 && idx < getSize()); return *reinterpret_cast(getElement(idx)); } @@ -374,13 +348,11 @@ class ArrayN : public ArrayBase * @param idx index of element. * @return reference to the specified element. */ - T &operator[](int idx) - { + T& operator[](int idx) { assert(idx >= 0 && idx < getSize()); return *reinterpret_cast(getElement(idx)); } }; -} // namespace cvcore - -#endif // CVCORE_ARRAY_H +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/BBox.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/BBox.h similarity index 88% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/BBox.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/BBox.h index 93100d3..37734f4 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/BBox.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/BBox.h @@ -15,21 +15,20 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_BBOX_H -#define CVCORE_BBOX_H +#pragma once #include #include #include namespace cvcore { +namespace tensor_ops { /** * A struct. * Structure used to store bounding box. */ -struct BBox -{ +struct BBox { int xmin{0}; /**< minimum x coordinate. */ int ymin{0}; /**< minimum y coordinate. */ int xmax{0}; /**< maximum x coordinate. */ @@ -40,8 +39,7 @@ struct BBox * @param Clamping bounding box (xmin, ymin, xmax, ymax) * @return Clamped bounding box */ - BBox clamp(const BBox &clampBox) const - { + BBox clamp(const BBox & clampBox) const { BBox outbox; outbox.xmin = std::max(clampBox.xmin, xmin); outbox.xmax = std::min(clampBox.xmax, xmax); @@ -53,24 +51,21 @@ struct BBox /** * @return Width of the bounding box */ - size_t getWidth() const - { + size_t getWidth() const { return xmax - xmin; } /** * @return Height of the bounding box */ - size_t getHeight() const - { + size_t getHeight() const { return ymax - ymin; } /** * Checks if the bounding box is valid. */ - bool isValid() const - { + bool isValid() const { return (xmin < xmax) && (ymin < ymax) && (getWidth() > 0) && (getHeight() > 0); } @@ -78,8 +73,7 @@ struct BBox * Returns the center of the bounding box * @return X,Y coordinate tuple */ - std::pair getCenter() const - { + std::pair getCenter() const { int centerX = xmin + getWidth() / 2; int centerY = ymin + getHeight() / 2; return std::pair(centerX, centerY); @@ -91,8 +85,7 @@ struct BBox * @param Scale in Y direction along the height * @return Scaled bounding box */ - BBox scale(float scaleW, float scaleH) const - { + BBox scale(float scaleW, float scaleH) const { auto center = getCenter(); float newW = getWidth() * scaleW; float newH = getHeight() * scaleH; @@ -111,22 +104,19 @@ struct BBox * @param Clamping bounding box (xmin, ymin, xmax, ymax) * @return Sqaure bounding box */ - BBox squarify(const BBox &clampBox) const - { + BBox squarify(const BBox & clampBox) const { size_t w = getWidth(); size_t h = getHeight(); BBox clampedBox1 = clamp(clampBox); - if (!clampedBox1.isValid()) - { + if (!clampedBox1.isValid()) { throw std::range_error("Invalid bounding box generated\n"); } float scaleW = static_cast(std::max(w, h)) / w; float scaleH = static_cast(std::max(w, h)) / h; BBox scaledBBox = clampedBox1.scale(scaleW, scaleH); BBox clampedBox2 = scaledBBox.clamp(clampBox); - if (!clampedBox2.isValid()) - { + if (!clampedBox2.isValid()) { throw std::range_error("Invalid bounding box generated\n"); } size_t newW = clampedBox2.getWidth(); @@ -138,5 +128,5 @@ struct BBox } }; -} // namespace cvcore -#endif // CVCORE_BBOX_H +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/BBoxUtils.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/BBoxUtils.cpp similarity index 56% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/BBoxUtils.cpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/BBoxUtils.cpp index 4559a35..7ddd105 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/BBoxUtils.cpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/BBoxUtils.cpp @@ -15,33 +15,29 @@ // // SPDX-License-Identifier: Apache-2.0 -#include "cv/tensor_ops/BBoxUtils.h" +#include "extensions/tensorops/core/BBoxUtils.h" #include #include -namespace cvcore { namespace tensor_ops { - +namespace cvcore { +namespace tensor_ops { namespace { -bool IsValid(const BBox &box) -{ +bool IsValid(const BBox& box) { return box.xmin >= 0 && box.ymin >= 0 && box.xmin < box.xmax && box.ymin < box.ymax; } -} // anonymous namespace +} // anonymous namespace -float GetArea(const BBox &box) -{ - if (box.xmax < box.xmin || box.ymax < box.ymin) - { +float GetArea(const BBox& box) { + if (box.xmax < box.xmin || box.ymax < box.ymin) { return 0.f; } return static_cast((box.xmax - box.xmin) * (box.ymax - box.ymin)); } -float GetIntersection(const BBox &a, const BBox &b) -{ +float GetIntersection(const BBox& a, const BBox& b) { const int lowerX = std::max(a.xmin, b.xmin); const int upperX = std::min(a.xmax, b.xmax); const int lowerY = std::max(a.ymin, b.ymin); @@ -51,20 +47,16 @@ float GetIntersection(const BBox &a, const BBox &b) return static_cast(diffX * diffY); } -float GetUnion(const BBox &a, const BBox &b) -{ +float GetUnion(const BBox& a, const BBox& b) { return GetArea(a) + GetArea(b) - GetIntersection(a, b); } -float GetIoU(const BBox &a, const BBox &b) -{ +float GetIoU(const BBox& a, const BBox& b) { return GetIntersection(a, b) / GetUnion(a, b); } -BBox MergeBoxes(const BBox &a, const BBox &b) -{ - if (!IsValid(a) || !IsValid(b)) - { +BBox MergeBoxes(const BBox& a, const BBox& b) { + if (!IsValid(a) || !IsValid(b)) { return IsValid(a) ? a : b; } BBox res; @@ -75,17 +67,16 @@ BBox MergeBoxes(const BBox &a, const BBox &b) return res; } -BBox ClampBox(const BBox &a, const BBox &b) -{ - return {std::max(a.xmin, b.xmin), std::max(a.ymin, b.ymin), std::min(a.xmax, b.xmax), std::min(a.ymax, b.ymax)}; +BBox ClampBox(const BBox& a, const BBox& b) { + return {std::max(a.xmin, b.xmin), std::max(a.ymin, b.ymin), + std::min(a.xmax, b.xmax), std::min(a.ymax, b.ymax)}; } -BBox InterpolateBoxes(float currLeft, float currRight, float currBottom, float currTop, float xScaler, float yScaler, - int currColumn, int currRow, BBoxInterpolationType type, float bboxNorm) -{ +BBox InterpolateBoxes(float currLeft, float currRight, float currBottom, + float currTop, float xScaler, float yScaler, + int currColumn, int currRow, BBoxInterpolationType type, float bboxNorm) { BBox currBoxInfo; - if (type == CONST_INTERPOLATION) - { + if (type == CONST_INTERPOLATION) { float centerX = ((currColumn * xScaler + 0.5) / bboxNorm); float centerY = ((currRow * yScaler + 0.5) / bboxNorm); float left = (currLeft - centerX); @@ -96,71 +87,61 @@ BBox InterpolateBoxes(float currLeft, float currRight, float currBottom, float c currBoxInfo.xmax = right * bboxNorm; currBoxInfo.ymin = top * -bboxNorm; currBoxInfo.ymax = bottom * bboxNorm; - } - else if (type == IMAGE_INTERPOLATION) - { - int centerX = (int)((currColumn + 0.5f) * xScaler); - int centerY = (int)((currRow + 0.5f) * yScaler); - int left = (int)(currLeft * xScaler); - int right = (int)(currRight * xScaler); - int top = (int)(currTop * yScaler); - int bottom = (int)(currBottom * yScaler); + } else if (type == IMAGE_INTERPOLATION) { + int centerX = static_cast((currColumn + 0.5f) * xScaler); + int centerY = static_cast((currRow + 0.5f) * yScaler); + int left = static_cast(currLeft * xScaler); + int right = static_cast(currRight * xScaler); + int top = static_cast(currTop * yScaler); + int bottom = static_cast(currBottom * yScaler); currBoxInfo.xmin = centerX - left; currBoxInfo.xmax = centerX + right; currBoxInfo.ymin = centerY - top; currBoxInfo.ymax = centerY + bottom; - } - else - { + } else { throw std::runtime_error("invalid bbox interpolation type"); } return currBoxInfo; } -BBox ScaleBox(const BBox &bbox, float xScaler, float yScaler, BBoxScaleType type) -{ +BBox ScaleBox(const BBox& bbox, float xScaler, float yScaler, BBoxScaleType type) { BBox output; - if (type == NORMAL) - { - int xMin = (int)(bbox.xmin * xScaler + 0.5f); - int yMin = (int)(bbox.ymin * yScaler + 0.5f); - int xMax = (int)(bbox.xmax * xScaler + 0.5f); - int yMax = (int)(bbox.ymax * yScaler + 0.5f); + if (type == NORMAL) { + int xMin = static_cast(bbox.xmin * xScaler + 0.5f); + int yMin = static_cast(bbox.ymin * yScaler + 0.5f); + int xMax = static_cast(bbox.xmax * xScaler + 0.5f); + int yMax = static_cast(bbox.ymax * yScaler + 0.5f); output = {xMin, yMin, xMax, yMax}; - } - else if (type == CENTER) - { + } else if (type == CENTER) { float xCenter = (bbox.xmax + bbox.xmin) / 2.0f; float yCenter = (bbox.ymax + bbox.ymin) / 2.0f; float width = (bbox.xmax - bbox.xmin) * xScaler; float height = (bbox.ymax - bbox.ymin) * yScaler; - output = {int(xCenter - width / 2 + 0.5f), int(yCenter - height / 2 + 0.5f), int(xCenter + width / 2 + 0.5f), - int(yCenter + height / 2 + 0.5f)}; - } - else - { + output = {static_cast(xCenter - width / 2 + 0.5f), + static_cast(yCenter - height / 2 + 0.5f), + static_cast(xCenter + width / 2 + 0.5f), + static_cast(yCenter + height / 2 + 0.5f)}; + } else { throw std::runtime_error("invalid bbox scaling type"); } return output; } -BBox TransformBox(const BBox &bbox, float xScaler, float yScaler, float xOffset, float yOffset) -{ - int xMin = (int)((bbox.xmin + xOffset) * xScaler + 0.5f); - int yMin = (int)((bbox.ymin + yOffset) * yScaler + 0.5f); - int xMax = (int)((bbox.xmax + xOffset) * xScaler + 0.5f); - int yMax = (int)((bbox.ymax + yOffset) * yScaler + 0.5f); +BBox TransformBox(const BBox& bbox, float xScaler, float yScaler, float xOffset, float yOffset) { + int xMin = static_cast((bbox.xmin + xOffset) * xScaler + 0.5f); + int yMin = static_cast((bbox.ymin + yOffset) * yScaler + 0.5f); + int xMax = static_cast((bbox.xmax + xOffset) * xScaler + 0.5f); + int yMax = static_cast((bbox.ymax + yOffset) * yScaler + 0.5f); return {xMin, yMin, xMax, yMax}; } -BBox SquarifyBox(const BBox &box, const BBox &boundary, float scale) -{ +BBox SquarifyBox(const BBox& box, const BBox& boundary, float scale) { BBox output = ClampBox(box, boundary); float updateWH = scale * std::max(output.xmax - output.xmin, output.ymax - output.ymin); - float scaleW = updateWH / float(output.xmax - output.xmin); - float scaleH = updateWH / float(output.ymax - output.ymin); + float scaleW = updateWH / static_cast(output.xmax - output.xmin); + float scaleH = updateWH / static_cast(output.ymax - output.ymin); output = ScaleBox(output, scaleW, scaleH, CENTER); output = ClampBox(output, boundary); @@ -170,4 +151,5 @@ BBox SquarifyBox(const BBox &box, const BBox &boundary, float scale) return {xmin, ymin, xmin + l, ymin + l}; } -}} // namespace cvcore::tensor_ops +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/BBoxUtils.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/BBoxUtils.h similarity index 78% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/BBoxUtils.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/BBoxUtils.h index 624139b..4c93b7b 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/BBoxUtils.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/BBoxUtils.h @@ -15,19 +15,18 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_BBOX_UTILS_H -#define CVCORE_BBOX_UTILS_H +#pragma once -#include "cv/core/BBox.h" +#include "extensions/tensorops/core/BBox.h" -namespace cvcore { namespace tensor_ops { +namespace cvcore { +namespace tensor_ops { /** * An enum. * Enum type for Bounding Box interpolation */ -enum BBoxInterpolationType -{ +enum BBoxInterpolationType { CONST_INTERPOLATION, /**< Uses constant value interpolation */ IMAGE_INTERPOLATION, /**< Interpolates based on image size */ }; @@ -36,8 +35,7 @@ enum BBoxInterpolationType * An enum. * Enum type for Bounding Box scaling operation. */ -enum BBoxScaleType -{ +enum BBoxScaleType { NORMAL, /**< Scale box without fixing center point. */ CENTER /**< Scale box with center fixed. */ }; @@ -48,7 +46,7 @@ enum BBoxScaleType * @param b the other BBox. * @return intersection area of the two bounding boxes. */ -float GetIntersection(const BBox &a, const BBox &b); +float GetIntersection(const BBox& a, const BBox& b); /** * Function to calculate the union of two bounding boxes. @@ -56,7 +54,7 @@ float GetIntersection(const BBox &a, const BBox &b); * @param b the other BBox. * @return union area of the two bounding boxes. */ -float GetUnion(const BBox &a, const BBox &b); +float GetUnion(const BBox& a, const BBox& b); /** * Function to calculate the IoU (Intersection over Union) of two bounding boxes. @@ -64,7 +62,7 @@ float GetUnion(const BBox &a, const BBox &b); * @param b the other BBox. * @return IoU of the two bounding boxes. */ -float GetIoU(const BBox &a, const BBox &b); +float GetIoU(const BBox& a, const BBox& b); /** * Function to merge two BBox together. @@ -72,7 +70,7 @@ float GetIoU(const BBox &a, const BBox &b); * @param b the other BBox. * @return Merged bounding box. */ -BBox MergeBoxes(const BBox &a, const BBox &b); +BBox MergeBoxes(const BBox& a, const BBox& b); /** * Clamp BBox. @@ -80,7 +78,7 @@ BBox MergeBoxes(const BBox &a, const BBox &b); * @param b boundary BBox. * @return clamped BBox. */ -BBox ClampBox(const BBox &a, const BBox &b); +BBox ClampBox(const BBox& a, const BBox& b); /** * Interpolate bounding boxes. @@ -96,9 +94,10 @@ BBox ClampBox(const BBox &a, const BBox &b); * @param bboxNorm bounding box scaled factor. * @return interpolated BBox. */ -BBox InterpolateBoxes(float currLeft, float currRight, float currBottom, float currTop, float xScaler, float yScaler, - int currColumn, int currRow, BBoxInterpolationType type = IMAGE_INTERPOLATION, - float bboxNorm = 1.0); +BBox InterpolateBoxes(float currLeft, float currRight, float currBottom, + float currTop, float xScaler, float yScaler, + int currColumn, int currRow, BBoxInterpolationType type = IMAGE_INTERPOLATION, + float bboxNorm = 1.0); /** * Scale BBox. @@ -108,7 +107,7 @@ BBox InterpolateBoxes(float currLeft, float currRight, float currBottom, float c * @param type BBox scaling type. * @return scaled BBox. */ -BBox ScaleBox(const BBox &bbox, float xScaler, float yScaler, BBoxScaleType type = NORMAL); +BBox ScaleBox(const BBox& bbox, float xScaler, float yScaler, BBoxScaleType type = NORMAL); /** * Transform BBox. @@ -119,7 +118,7 @@ BBox ScaleBox(const BBox &bbox, float xScaler, float yScaler, BBoxScaleType type * @param yOffset offset along height direction in pixels. * @return transformed BBox. */ -BBox TransformBox(const BBox &bbox, float xScaler, float yScaler, float xOffset, float yOffset); +BBox TransformBox(const BBox& bbox, float xScaler, float yScaler, float xOffset, float yOffset); /** * Squarify BBox. @@ -128,8 +127,7 @@ BBox TransformBox(const BBox &bbox, float xScaler, float yScaler, float xOffset, * @param scale scaling factor. * @return squarified BBox. */ -BBox SquarifyBox(const BBox &box, const BBox &boundary, float scale); +BBox SquarifyBox(const BBox& box, const BBox& boundary, float scale); -}} // namespace cvcore::tensor_ops - -#endif // CVCORE_BBOX_UTILS_H \ No newline at end of file +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/CVError.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/CVError.cpp new file mode 100644 index 0000000..4886ccc --- /dev/null +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/CVError.cpp @@ -0,0 +1,116 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 + +#include "extensions/tensorops/core/CVError.h" + +#include +#include +#include + +#ifndef __cpp_lib_to_underlying +// Using a C++23 feature by hacking std +namespace std { + template + constexpr underlying_type_t to_underlying(Enum e) noexcept { + return static_cast>(e); + } +}; +#endif // __cpp_lib_to_underlying + +namespace cvcore { +namespace tensor_ops { + +namespace detail { +struct CoreErrorCategory : std::error_category { + const char * name() const noexcept final { + return "cvcore-error"; + } + + std::string message(int value) const final { + std::string result; + + switch (value) { + case std::to_underlying(ErrorCode::SUCCESS): + result = "(SUCCESS) No errors detected"; + break; + case std::to_underlying(ErrorCode::NOT_READY): + result = "(NOT_READY) The execution of the requested " + "operation is not to return"; + break; + case std::to_underlying(ErrorCode::NOT_IMPLEMENTED): + result = "(NOT_IMPLEMENTED) The requested operation is not " + "implemented"; + break; + case std::to_underlying(ErrorCode::INVALID_ARGUMENT): + result = "(INVALID_ARGUMENT) The argument provided to the " + "operation is not currently supported"; + break; + case std::to_underlying(ErrorCode::INVALID_IMAGE_FORMAT): + result = "(INVALID_IMAGE_FORMAT) The requested image format " + "is not supported by the operation"; + break; + case std::to_underlying(ErrorCode::INVALID_STORAGE_TYPE): + result = "(INVALID_STORAGE_TYPE) The requested storage type " + "is not supported by the operation"; + break; + case std::to_underlying(ErrorCode::INVALID_ENGINE_TYPE): + result = "(INVALID_ENGINE_TYPE) The requested engine type " + "is not supported by the operation"; + break; + case std::to_underlying(ErrorCode::INVALID_OPERATION): + result = "(INVALID_OPERATION) The requested operation is " + "not supported"; + break; + case std::to_underlying(ErrorCode::DETECTED_NAN_IN_RESULT): + result = "(DETECTED_NAN_IN_RESULT) NaN was detected in the " + "return value of the operation"; + break; + case std::to_underlying(ErrorCode::OUT_OF_MEMORY): + result = "(OUT_OF_MEMORY) The device has run out of memory"; + break; + case std::to_underlying(ErrorCode::DEVICE_ERROR): + result = "(DEVICE_ERROR) A device level error has been " + "encountered"; + break; + case std::to_underlying(ErrorCode::SYSTEM_ERROR): + result = "(SYSTEM_ERROR) A system level error has been " + "encountered"; + break; + default: + result = "(Unrecognized Condition) Value " + std::to_string(value) + + " does not map to known error code literal " + + " defined by cvcore::tensor_ops::ErrorCode"; + break; + } + + return result; + } +}; +} // namespace detail + +const detail::CoreErrorCategory errorCategory{}; + +std::error_condition make_error_condition(ErrorCode ec) noexcept { + return {std::to_underlying(ec), errorCategory}; +} + +std::error_code make_error_code(ErrorCode ec) noexcept { + return {std::to_underlying(ec), errorCategory}; +} + +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/CVError.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/CVError.h similarity index 64% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/CVError.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/CVError.h index 82c16c1..0467aa0 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/CVError.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/CVError.h @@ -15,18 +15,17 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_CVERROR_H -#define CVCORE_CVERROR_H +#pragma once +#include #include #include #include #include #include -#include - namespace cvcore { +namespace tensor_ops { // CVCORE ERROR CODES // ----------------------------------------------------------------------------- @@ -34,12 +33,11 @@ namespace cvcore { // std::error_condition creates a set of sub-system independent codes which may // be used to describe ANY downstream error in a broad sense. An std::error_code // is defined within the sub-system context (i.e. tensor_ops, trtbackend, ...) -// which is mapped to the cvcore::ErrorCode. -// As an example, cvcore::ErrorCode -1 may not ABSOLUTELY mean the same as +// which is mapped to the cvcore::tensor_ops::ErrorCode. +// As an example, cvcore::tensor_ops::ErrorCode -1 may not ABSOLUTELY mean the same as // tensor_ops::FaultCode -1, but does mean the same as tensor_ops:FaultCode 4. -// Thus, tensor_ops::FaultCode 4 needs to be mapped to cvcore::ErrorCode -1. -enum class ErrorCode : std::int32_t -{ +// Thus, tensor_ops::FaultCode 4 needs to be mapped to cvcore::tensor_ops::ErrorCode -1. +enum class ErrorCode : std::int32_t { SUCCESS = 0, NOT_READY, NOT_IMPLEMENTED, @@ -54,24 +52,23 @@ enum class ErrorCode : std::int32_t SYSTEM_ERROR, }; -} // namespace cvcore +} // namespace tensor_ops +} // namespace cvcore // WARNING: Extending base C++ namespace to cover cvcore error codes namespace std { - template<> -struct is_error_condition_enum : true_type -{ +struct is_error_condition_enum : true_type { }; template<> -struct is_error_code_enum : true_type -{ +struct is_error_code_enum : true_type { }; -} // namespace std +} // namespace std namespace cvcore { +namespace tensor_ops { std::error_condition make_error_condition(ErrorCode) noexcept; @@ -79,38 +76,33 @@ std::error_code make_error_code(ErrorCode) noexcept; // ----------------------------------------------------------------------------- -inline void CheckCudaError(cudaError_t code, const char *file, const int line) -{ - if (code != cudaSuccess) - { - const char *errorMessage = cudaGetErrorString(code); - const std::string message = "CUDA error returned at " + std::string(file) + ":" + std::to_string(line) + - ", Error code: " + std::to_string(code) + " (" + std::string(errorMessage) + ")"; +inline void CheckCudaError(cudaError_t code, const char * file, const int line) { + if (code != cudaSuccess) { + const char* errorMessage = cudaGetErrorString(code); + const std::string message = "CUDA error returned at " + std::string(file) + + ":" + std::to_string(line) + ", Error code: " + std::to_string(code) + + " (" + std::string(errorMessage) + ")"; throw std::runtime_error(message); } } -inline void CheckErrorCode(std::error_code err, const char *file, const int line) -{ - const std::string message = "Error returned at " + std::string(file) + ":" + std::to_string(line) + - ", Error code: " + std::string(err.message()); - - if (err != cvcore::make_error_code(cvcore::ErrorCode::SUCCESS)) - { +inline void CheckErrorCode(std::error_code err, const char * file, const int line) { + const std::string message = "Error returned at " + std::string(file) + ":" + + std::to_string(line) + ", Error code: " + std::string(err.message()); + if (err != cvcore::tensor_ops::make_error_code(cvcore::tensor_ops::ErrorCode::SUCCESS)) { throw std::runtime_error(message); } } -} // namespace cvcore +} // namespace tensor_ops +} // namespace cvcore #define CHECK_ERROR(val) \ { \ - cvcore::CheckCudaError((val), __FILE__, __LINE__); \ + cvcore::tensor_ops::CheckCudaError((val), __FILE__, __LINE__); \ } #define CHECK_ERROR_CODE(val) \ { \ - cvcore::CheckErrorCode((val), __FILE__, __LINE__); \ + cvcore::tensor_ops::CheckErrorCode((val), __FILE__, __LINE__); \ } - -#endif // CVCORE_CVERROR_H diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/CameraModel.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/CameraModel.h similarity index 83% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/CameraModel.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/CameraModel.h index 157acf4..cbfccbc 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/CameraModel.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/CameraModel.h @@ -15,22 +15,22 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_CAMERAMODEL_H -#define CVCORE_CAMERAMODEL_H +#pragma once +#include #include -#include "cv/core/Array.h" -#include "cv/core/MathTypes.h" +#include "extensions/tensorops/core/Array.h" +#include "extensions/tensorops/core/MathTypes.h" namespace cvcore { +namespace tensor_ops { /** * An enum. * Enum type for Camera Distortion type. */ -enum class CameraDistortionType : uint8_t -{ +enum class CameraDistortionType : uint8_t { UNKNOWN, /**< Unknown arbitrary distortion model. */ NONE, /**< No distortion applied. */ Polynomial, /**< Polynomial distortion model. */ @@ -43,14 +43,12 @@ enum class CameraDistortionType : uint8_t /** * Struct type used to store Camera Distortion model type and coefficients. */ -struct CameraDistortionModel -{ +struct CameraDistortionModel { CameraDistortionType type; /**< Camera distortion model type. */ - union /**< Camera distortion model coefficients. */ - { + /**< Camera distortion model coefficients. */ + union { float coefficients[8]; - struct - { + struct { float k1, k2, k3, k4, k5, k6; float p1, p2; }; @@ -67,10 +65,11 @@ struct CameraDistortionModel * @param distortionCoefficients An array of camera distortion model coefficients * @return Camera Distortion Model */ - CameraDistortionModel(CameraDistortionType distortionType, std::array & distortionCoefficients) - : type(distortionType) - { - std::copy(distortionCoefficients.begin(), distortionCoefficients.end(), std::begin(coefficients)); + CameraDistortionModel(CameraDistortionType distortionType, + std::array & distortionCoefficients) + : type(distortionType) { + std::copy(distortionCoefficients.begin(), distortionCoefficients.end(), + std::begin(coefficients)); } /** @@ -96,17 +95,14 @@ struct CameraDistortionModel , k5(k5) , k6(k6) , p1(p1) - , p2(p2) - { - + , p2(p2) { } /** * Get camera distortion model type. * @return Camera distortion model type */ - CameraDistortionType getDistortionType() const - { + CameraDistortionType getDistortionType() const { return type; } @@ -114,25 +110,22 @@ struct CameraDistortionModel * Get camera distortion model coefficients. * @return Camera distortion model coefficients array */ - const float * getCoefficients() const - { + const float * getCoefficients() const { return &coefficients[0]; } - inline bool operator==(const CameraDistortionModel & other) const noexcept - { + inline bool operator==(const CameraDistortionModel & other) const noexcept { return this->k1 == other.k1 && - this->k2 == other.k2 && - this->k3 == other.k3 && - this->k4 == other.k4 && - this->k5 == other.k5 && - this->k6 == other.k6 && - this->p1 == other.p1 && + this->k2 == other.k2 && + this->k3 == other.k3 && + this->k4 == other.k4 && + this->k5 == other.k5 && + this->k6 == other.k6 && + this->p1 == other.p1 && this->p2 == other.p2; } - inline bool operator!=(const CameraDistortionModel & other) const noexcept - { + inline bool operator!=(const CameraDistortionModel & other) const noexcept { return !(*this == other); } }; @@ -140,8 +133,7 @@ struct CameraDistortionModel /** * Struct type used to store Camera Intrinsics. */ -struct CameraIntrinsics -{ +struct CameraIntrinsics { CameraIntrinsics() = default; /** @@ -153,8 +145,7 @@ struct CameraIntrinsics * @param s Camera slanted pixel * @return Camera Intrinsics */ - CameraIntrinsics(float fx, float fy, float cx, float cy, float s = 0.0) - { + CameraIntrinsics(float fx, float fy, float cx, float cy, float s = 0.0) { m_intrinsics[0][0] = fx; m_intrinsics[0][1] = s; m_intrinsics[0][2] = cx; @@ -167,8 +158,7 @@ struct CameraIntrinsics * Get camera intrinsics x focal length. * @return Camera x focal length */ - float fx() const - { + float fx() const { return m_intrinsics[0][0]; } @@ -176,8 +166,7 @@ struct CameraIntrinsics * Get camera intrinsics y focal length. * @return Camera y focal length */ - float fy() const - { + float fy() const { return m_intrinsics[1][1]; } @@ -185,8 +174,7 @@ struct CameraIntrinsics * Get camera intrinsics x principal point. * @return Camera x principal point */ - float cx() const - { + float cx() const { return m_intrinsics[0][2]; } @@ -194,8 +182,7 @@ struct CameraIntrinsics * Get camera intrinsics y principal point. * @return Camera y principal point */ - float cy() const - { + float cy() const { return m_intrinsics[1][2]; } @@ -203,8 +190,7 @@ struct CameraIntrinsics * Get camera intrinsics slanted pixels. * @return Camera slanted pixels */ - float skew() const - { + float skew() const { return m_intrinsics[0][1]; } @@ -212,13 +198,11 @@ struct CameraIntrinsics * Get camera intrinsics 2D array. * @return Camera intrisics array */ - const float * getMatrix23() const - { + const float * getMatrix23() const { return &m_intrinsics[0][0]; } - inline bool operator==(const CameraIntrinsics & other) const noexcept - { + inline bool operator==(const CameraIntrinsics & other) const noexcept { return m_intrinsics[0][0] == other.m_intrinsics[0][0] && m_intrinsics[0][1] == other.m_intrinsics[0][1] && m_intrinsics[0][2] == other.m_intrinsics[0][2] && @@ -227,19 +211,17 @@ struct CameraIntrinsics m_intrinsics[1][2] == other.m_intrinsics[1][2]; } - inline bool operator!=(const CameraIntrinsics & other) const noexcept - { + inline bool operator!=(const CameraIntrinsics & other) const noexcept { return !(*this == other); } - - float m_intrinsics[2][3] {{1.0, 0.0, 0.0},{0.0, 1.0, 0.0}}; /**< Camera intrinsics 2D arrat. */ + /**< Camera intrinsics 2D array. */ + float m_intrinsics[2][3] {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}}; }; /** * Struct type used to store Camera Extrinsics. */ -struct CameraExtrinsics -{ +struct CameraExtrinsics { using RawMatrixType = float[3][4]; CameraExtrinsics() = default; @@ -249,13 +231,11 @@ struct CameraExtrinsics * @param extrinsics Camera extrinsics as raw 2D array * @return Camera Extrinsics */ - explicit CameraExtrinsics(const RawMatrixType & extrinsics) - { + explicit CameraExtrinsics(const RawMatrixType & extrinsics) { std::copy(&extrinsics[0][0], &extrinsics[0][0] + 3 * 4, &m_extrinsics[0][0]); } - inline bool operator==(const CameraExtrinsics & other) const noexcept - { + inline bool operator==(const CameraExtrinsics & other) const noexcept { return m_extrinsics[0][0] == other.m_extrinsics[0][0] && m_extrinsics[0][1] == other.m_extrinsics[0][1] && m_extrinsics[0][2] == other.m_extrinsics[0][2] && @@ -270,8 +250,7 @@ struct CameraExtrinsics m_extrinsics[2][3] == other.m_extrinsics[2][3]; } - inline bool operator!=(const CameraExtrinsics & other) const noexcept - { + inline bool operator!=(const CameraExtrinsics & other) const noexcept { return !(*this == other); } @@ -280,13 +259,11 @@ struct CameraExtrinsics {0.0, 0.0, 1.0, 0.0}}; }; -struct CameraModel -{ +struct CameraModel { CameraIntrinsics intrinsic; CameraExtrinsics extrinsic; CameraDistortionModel distortion; }; -} // namespace cvcore - -#endif // CVCORE_CAMERAMODEL_H +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/ColorConversions.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/ColorConversions.cpp new file mode 100644 index 0000000..b7d9a95 --- /dev/null +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/ColorConversions.cpp @@ -0,0 +1,559 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2020-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +#include "extensions/tensorops/core/ImageUtils.h" +#include "extensions/tensorops/core/Memory.h" +#include "extensions/tensorops/core/NppUtils.h" +#include "nppi_arithmetic_and_logical_operations.h" +#include "nppi_color_conversion.h" +#include "nppi_data_exchange_and_initialization.h" +namespace cvcore { +namespace tensor_ops { + +const float BGR2GRAY_COEFFS[3] = {0.114f, 0.587f, 0.299f}; +const float RGB2GRAY_COEFFS[3] = {0.299f, 0.587f, 0.114f}; + +template +bool ConvertColorFormatBatch(Tensor& dst, + Tensor& src, ColorConversionType type, + cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU() || (src.getDepth() != dst.getDepth())) { + return false; + } + for (size_t i = 0; i < src.getDepth(); i++) { + size_t offsetSrc = i * src.getStride(TensorDimension::DEPTH); + size_t offsetDst = i * dst.getStride(TensorDimension::DEPTH); + Tensor srcTmp(src.getWidth(), src.getHeight(), + src.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), + src.getData() + offsetSrc, false); + Tensor dstTmp(dst.getWidth(), dst.getHeight(), + dst.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), + dst.getData() + offsetDst, false); + bool ret = ConvertColorFormat(dstTmp, srcTmp, type, stream); + if (!ret) { + return false; + } + } + return true; +} + +template +bool InterleavedToPlanarBatch(Tensor& dst, + Tensor& src, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU() || (src.getDepth() != dst.getDepth())) { + return false; + } + + for (size_t i = 0; i < src.getDepth(); i++) { + size_t offsetSrc = i * src.getStride(TensorDimension::DEPTH); + size_t offsetDst = i * dst.getStride(TensorDimension::DEPTH); + Tensor srcTmp(src.getWidth(), src.getHeight(), + src.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), + src.getData() + offsetSrc, false); + Tensor dstTmp(dst.getWidth(), dst.getHeight(), + dst.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), + dst.getData() + offsetDst, false); + bool ret = InterleavedToPlanar(dstTmp, srcTmp, stream); + if (!ret) { + return false; + } + } + return true; +} + +bool ConvertColorFormat(Tensor& dst, const Tensor& src, + ColorConversionType type, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if ((src.getWidth() != dst.getWidth()) || (src.getHeight() != dst.getHeight())) { + return false; + } + if (type == BGR2RGB || type == RGB2BGR) { + const int order[3] = {2, 1, 0}; + NppStatus status = nppiSwapChannels_8u_C3R_Ctx( + static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + {static_cast(src.getWidth()), static_cast(src.getHeight())}, + order, GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + } else { + return false; + } + return true; +} + +bool ConvertColorFormat(Tensor& dst, const Tensor& src, + ColorConversionType type, cudaStream_t stream) { + return ConvertColorFormatBatch(dst, const_cast &>(src), type, stream); +} + +bool ConvertColorFormat(Tensor& dst, const Tensor& src, + ColorConversionType type, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if ((src.getWidth() != dst.getWidth()) || (src.getHeight() != dst.getHeight())) { + return false; + } + if (type == BGR2RGB || type == RGB2BGR) { + const int order[3] = {2, 1, 0}; + NppStatus status = nppiSwapChannels_16u_C3R_Ctx( + static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + {static_cast(src.getWidth()), static_cast(src.getHeight())}, + order, GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + } else { + return false; + } + return true; +} + +bool ConvertColorFormat(Tensor& dst, const Tensor& src, + ColorConversionType type, cudaStream_t stream) { + return ConvertColorFormatBatch(dst, const_cast &>(src), type, stream); +} + +bool ConvertColorFormat(Tensor& dst, const Tensor& src, + ColorConversionType type, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if ((src.getWidth() != dst.getWidth()) || (src.getHeight() != dst.getHeight())) { + return false; + } + + if (type == BGR2RGB || type == RGB2BGR) { + const int order[3] = {2, 1, 0}; + NppStatus status = nppiSwapChannels_32f_C3R_Ctx( + static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + {static_cast(src.getWidth()), static_cast(src.getHeight())}, order, + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + } else { + return false; + } + return true; +} + +bool ConvertColorFormat(Tensor& dst, const Tensor& src, + ColorConversionType type, cudaStream_t stream) { + return ConvertColorFormatBatch(dst, const_cast &>(src), type, stream); +} + +bool ConvertColorFormat(Tensor& dst, const Tensor& src, + ColorConversionType type, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if ((src.getWidth() != dst.getWidth()) || (src.getHeight() != dst.getHeight())) { + return false; + } + + if (type == BGR2GRAY || type == RGB2GRAY) { + NppStatus status = nppiColorToGray_8u_C3C1R_Ctx( + static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + {static_cast(src.getWidth()), static_cast(src.getHeight())}, + type == BGR2GRAY ? BGR2GRAY_COEFFS : RGB2GRAY_COEFFS, + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + } else { + return false; + } + return true; +} + +bool ConvertColorFormat(Tensor& dst, const Tensor& src, + ColorConversionType type, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if ((src.getWidth() != dst.getWidth()) || (src.getHeight() != dst.getHeight())) { + return false; + } + + if (type == BGR2GRAY || type == RGB2GRAY) { + NppStatus status = nppiColorToGray_16u_C3C1R_Ctx( + static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + {static_cast(src.getWidth()), static_cast(src.getHeight())}, + type == BGR2GRAY ? BGR2GRAY_COEFFS : RGB2GRAY_COEFFS, + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + } else { + return false; + } + return true; +} + +bool ConvertColorFormat(Tensor& dst, const Tensor& src, + ColorConversionType type, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if ((src.getWidth() != dst.getWidth()) || (src.getHeight() != dst.getHeight())) { + return false; + } + + if (type == BGR2GRAY || type == RGB2GRAY) { + NppStatus status = nppiColorToGray_32f_C3C1R_Ctx( + static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + {static_cast(src.getWidth()), static_cast(src.getHeight())}, + type == BGR2GRAY ? BGR2GRAY_COEFFS : RGB2GRAY_COEFFS, + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + } else { + return false; + } + return true; +} + +bool ConvertColorFormat(Tensor& dst, const Tensor& src, + ColorConversionType type, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if ((src.getWidth() != dst.getWidth()) || (src.getHeight() != dst.getHeight())) { + return false; + } + + if (type == GRAY2BGR || type == GRAY2RGB) { + NppStatus status = nppiDup_8u_C1C3R_Ctx( + static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + {static_cast(src.getWidth()), static_cast(src.getHeight())}, + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + } else { + return false; + } + return true; +} + +bool ConvertColorFormat(Tensor& dst, const Tensor& src, + ColorConversionType type, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if ((src.getWidth() != dst.getWidth()) || (src.getHeight() != dst.getHeight())) { + return false; + } + + if (type == GRAY2BGR || type == GRAY2RGB) { + NppStatus status = nppiDup_16u_C1C3R_Ctx( + static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + {static_cast(src.getWidth()), static_cast(src.getHeight())}, + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + } else { + return false; + } + return true; +} + +bool ConvertColorFormat(Tensor& dst, const Tensor& src, + ColorConversionType type, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if ((src.getWidth() != dst.getWidth()) || (src.getHeight() != dst.getHeight())) { + return false; + } + + if (type == GRAY2BGR || type == GRAY2RGB) { + NppStatus status = nppiDup_32f_C1C3R_Ctx( + static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + {static_cast(src.getWidth()), static_cast(src.getHeight())}, + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + } else { + return false; + } + return true; +} + +bool ConvertBitDepth(Tensor& dst, Tensor& src, + const float scale, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if ((src.getWidth() != dst.getWidth()) || (src.getHeight() != dst.getHeight())) { + return false; + } + + const NppiSize srcSize = {static_cast(src.getWidth()), static_cast(src.getHeight())}; + + NppStreamContext streamContext = GetNppStreamContext(stream); + + NppStatus status = + nppiMulC_32f_C1IR_Ctx(static_cast(scale), static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + srcSize, streamContext); + if (status != NPP_SUCCESS) { + return false; + } + + status = nppiConvert_32f8u_C1R_Ctx( + static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + {static_cast(src.getWidth()), static_cast(src.getHeight())}, + NPP_RND_FINANCIAL, streamContext); + if (status != NPP_SUCCESS) { + return false; + } + + return true; +} + +bool ConvertBitDepth(Tensor& dst, Tensor& src, + const float scale, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU() || (src.getDepth() != dst.getDepth())) { + return false; + } + + Tensor srcTmp(src.getWidth(), src.getDepth() * src.getHeight(), + src.getStride(TensorDimension::HEIGHT) * GetChannelSize(F32), + src.getData(), false); + Tensor dstTmp(dst.getWidth(), dst.getDepth() * dst.getHeight(), + dst.getStride(TensorDimension::HEIGHT) * GetChannelSize(U8), dst.getData(), false); + return ConvertBitDepth(dstTmp, srcTmp, scale, stream); +} + +bool InterleavedToPlanar(Tensor& dst, + const Tensor& src, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if ((src.getWidth() != dst.getWidth()) || (src.getHeight() != dst.getHeight())) { + return false; + } + + NppStatus status; + NppStreamContext streamContext = GetNppStreamContext(stream); + + const size_t offset = dst.getStride(TensorDimension::HEIGHT) * dst.getHeight(); + Npp8u *const dstBuffer[3] = {dst.getData(), dst.getData() + offset, + dst.getData() + 2 * offset}; + status = nppiCopy_8u_C3P3R_Ctx(static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), dstBuffer, + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + {static_cast(src.getWidth()), static_cast(src.getHeight())}, + streamContext); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +bool InterleavedToPlanar(Tensor& dst, const Tensor& src, + cudaStream_t stream) { + return InterleavedToPlanarBatch(dst, const_cast &>(src), stream); +} + +bool InterleavedToPlanar(Tensor& dst, const Tensor& src, + cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if ((src.getWidth() != dst.getWidth()) || (src.getHeight() != dst.getHeight())) { + return false; + } + + NppStatus status; + NppStreamContext streamContext = GetNppStreamContext(stream); + + const size_t offset = dst.getStride(TensorDimension::HEIGHT) * dst.getHeight(); + Npp16u *const dstBuffer[3] = {dst.getData(), dst.getData() + offset, + dst.getData() + 2 * offset}; + status = nppiCopy_16u_C3P3R_Ctx(static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), dstBuffer, + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + {static_cast(src.getWidth()), static_cast(src.getHeight())}, + streamContext); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +bool InterleavedToPlanar(Tensor& dst, const Tensor& src, + cudaStream_t stream) { + return InterleavedToPlanarBatch(dst, const_cast &>(src), stream); +} + +bool InterleavedToPlanar(Tensor& dst, const Tensor& src, + cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if ((src.getWidth() != dst.getWidth()) || (src.getHeight() != dst.getHeight())) { + return false; + } + + NppStatus status; + NppStreamContext streamContext = GetNppStreamContext(stream); + + const size_t offset = dst.getStride(TensorDimension::HEIGHT) * dst.getHeight(); + Npp32f *const dstBuffer[3] = {dst.getData(), dst.getData() + offset, + dst.getData() + 2 * offset}; + status = nppiCopy_32f_C3P3R_Ctx(static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), dstBuffer, + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + {static_cast(src.getWidth()), static_cast(src.getHeight())}, + streamContext); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +bool InterleavedToPlanar(Tensor& dst, const Tensor& src, + cudaStream_t stream) { + return InterleavedToPlanarBatch(dst, const_cast &>(src), stream); +} + +bool InterleavedToPlanar(Tensor& dst, const Tensor& src, + cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if ((src.getWidth() != dst.getWidth()) || (src.getHeight() != dst.getHeight())) { + return false; + } + + Tensor tmp(dst.getWidth(), dst.getHeight(), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + dst.getData(), false); + Copy(tmp, src, stream); + return true; +} + +bool InterleavedToPlanar(Tensor& dst, const Tensor& src, + cudaStream_t stream) { + return InterleavedToPlanarBatch(dst, const_cast &>(src), stream); +} + +bool InterleavedToPlanar(Tensor& dst, const Tensor& src, + cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if ((src.getWidth() != dst.getWidth()) || (src.getHeight() != dst.getHeight())) { + return false; + } + + Tensor tmp(dst.getWidth(), dst.getHeight(), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + dst.getData(), false); + Copy(tmp, src, stream); + return true; +} + +bool InterleavedToPlanar(Tensor& dst, const Tensor& src, + cudaStream_t stream) { + return InterleavedToPlanarBatch(dst, const_cast &>(src), stream); +} + +bool InterleavedToPlanar(Tensor& dst, const Tensor& src, + cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if ((src.getWidth() != dst.getWidth()) || (src.getHeight() != dst.getHeight())) { + return false; + } + + Tensor tmp(dst.getWidth(), dst.getHeight(), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + dst.getData(), false); + Copy(tmp, src, stream); + return true; +} + +bool InterleavedToPlanar(Tensor& dst, const Tensor& src, + cudaStream_t stream) { + return InterleavedToPlanarBatch(dst, const_cast &>(src), stream); +} + +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/ComputeEngine.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/ComputeEngine.h similarity index 61% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/ComputeEngine.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/ComputeEngine.h index 65fe7ca..9207a07 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/ComputeEngine.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/ComputeEngine.h @@ -15,29 +15,27 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_COMPUTEENGINE_H -#define CVCORE_COMPUTEENGINE_H +#pragma once #include namespace cvcore { +namespace tensor_ops { -enum class ComputeEngine : unsigned int -{ - UNKNOWN = 0x00, // 0000_0000 +enum class ComputeEngine : unsigned int { + UNKNOWN = 0x00, // 0000_0000 - CPU = 0x01, // 0000_0001 - PVA = 0x02, // 0000_0010 - VIC = 0x04, // 0000_0100 - NVENC = 0x08, // 0000_1000 - GPU = 0x10, // 0001_0000 - DLA = 0x20, // 0010_0000 - DLA_CORE_0 = 0x40, // 0100_0000 - DLA_CORE_1 = 0x80, // 1000_0000 + CPU = 0x01, // 0000_0001 + PVA = 0x02, // 0000_0010 + VIC = 0x04, // 0000_0100 + NVENC = 0x08, // 0000_1000 + GPU = 0x10, // 0001_0000 + DLA = 0x20, // 0010_0000 + DLA_CORE_0 = 0x40, // 0100_0000 + DLA_CORE_1 = 0x80, // 1000_0000 COMPUTE_FAULT = 0xFF // 1111_1111 }; -} // namespace cvcore - -#endif // CVCORE_COMPUTEENGINE_H +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/Core.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Core.h similarity index 64% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/Core.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Core.h index 42732d9..072d24f 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/Core.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Core.h @@ -15,21 +15,12 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CORE_H -#define CORE_H +#pragma once namespace cvcore { +namespace tensor_ops { -// Enable dll imports/exports in case of windows support -#ifdef _WIN32 -#ifdef CVCORE_EXPORT_SYMBOLS // Needs to be enabled in case of compiling dll -#define CVCORE_API __declspec(dllexport) // Exports symbols when compiling the library. -#else -#define CVCORE_API __declspec(dllimport) // Imports the symbols when linked with library. -#endif -#else #define CVCORE_API -#endif -} // namespace cvcore -#endif // CORE_H +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/DBScan.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/DBScan.cpp similarity index 68% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/DBScan.cpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/DBScan.cpp index f877154..ed27cd7 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/DBScan.cpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/DBScan.cpp @@ -15,13 +15,14 @@ // // SPDX-License-Identifier: Apache-2.0 -#include "cv/tensor_ops/DBScan.h" -#include "cv/tensor_ops/BBoxUtils.h" - #include #include -namespace cvcore { namespace tensor_ops { +#include "extensions/tensorops/core/BBoxUtils.h" +#include "extensions/tensorops/core/DBScan.h" + +namespace cvcore { +namespace tensor_ops { constexpr int kUnclassified = -1; constexpr int kCorePoint = 1; @@ -30,35 +31,30 @@ constexpr int kNoise = -2; namespace { -float CalculateDistance(const BBox &lhs, const BBox &rhs) -{ +float CalculateDistance(const BBox& lhs, const BBox& rhs) { const float iou = GetIoU(lhs, rhs); return 1.0f - iou; } -void MergeMaximumBBoxes(Array &input, Array &clusters, Array &output) -{ +void MergeMaximumBBoxes(Array& input, Array& clusters, Array& output) { BBox tempBox = {-1, -1, -1, -1}; // Initialize each cluster-box with a placeholder that has no cluster - for (int i = 0; i < output.getSize(); i++) - { + for (size_t i = 0; i < output.getSize(); i++) { // It's a struct so these pushes are by value output[i] = tempBox; } - for (int i = 0; i < input.getSize(); i++) - { + for (size_t i = 0; i < input.getSize(); i++) { int clusterId = clusters[i]; - if (clusterId >= 0) - { + if (clusterId >= 0) { // Box merging is associative & commutative output[clusterId] = MergeBoxes(input[i], output[clusterId]); } } } -void MergeWeightedBBoxes(Array &input, Array &clusters, Array &weights, Array &output) -{ +void MergeWeightedBBoxes(Array& input, Array& clusters, + Array& weights, Array& output) { int numClusters = output.getSize(); // centos has gcc 4.8.5 which complains about initializing variable sized arrays with {}. // Use std::vector for variable sized array. @@ -68,11 +64,9 @@ void MergeWeightedBBoxes(Array &input, Array &clusters, Array std::vector ymaxs(numClusters, 0); std::vector scales(numClusters, 0); - for (int i = 0; i < input.getSize(); i++) - { + for (size_t i = 0; i < input.getSize(); i++) { int clusterId = clusters[i]; - if (clusterId >= 0) - { + if (clusterId >= 0) { xmins[clusterId] += input[i].xmin * weights[i]; ymins[clusterId] += input[i].ymin * weights[i]; xmaxs[clusterId] += input[i].xmax * weights[i]; @@ -81,65 +75,55 @@ void MergeWeightedBBoxes(Array &input, Array &clusters, Array } } - for (int i = 0; i < numClusters; i++) - { - output[i] = {int(xmins[i] / scales[i] + 0.5f), int(ymins[i] / scales[i] + 0.5f), - int(xmaxs[i] / scales[i] + 0.5f), int(ymaxs[i] / scales[i] + 0.5f)}; + for (int i = 0; i < numClusters; i++) { + output[i] = {static_cast(xmins[i] / scales[i] + 0.5f), + static_cast(ymins[i] / scales[i] + 0.5f), + static_cast(xmaxs[i] / scales[i] + 0.5f), + static_cast(ymaxs[i] / scales[i] + 0.5f)}; } } -} // anonymous namespace +} // anonymous namespace DBScan::DBScan(int pointsSize, int minPoints, float epsilon) : m_pointsSize(pointsSize) , m_numClusters(0) , m_minPoints(minPoints) , m_epsilon(epsilon) - , m_clusterStates(pointsSize, true) -{ + , m_clusterStates(pointsSize, true) { m_clusterStates.setSize(pointsSize); } -void DBScan::doCluster(Array &input, Array &clusters) -{ +void DBScan::doCluster(Array& input, Array& clusters) { // Reset all cluster id - for (int i = 0; i < m_pointsSize; i++) - { + for (int i = 0; i < m_pointsSize; i++) { clusters[i] = -1; m_clusterStates[i] = kUnclassified; } int nextClusterId = 0; - for (int cIndex = 0; cIndex < m_pointsSize; cIndex++) - { + for (int cIndex = 0; cIndex < m_pointsSize; cIndex++) { std::vector neighbors; - for (int neighborIndex = 0; neighborIndex < m_pointsSize; neighborIndex++) - { - if (neighborIndex == cIndex) - { - continue; // Don't look at being your own neighbor + for (int neighborIndex = 0; neighborIndex < m_pointsSize; neighborIndex++) { + if (neighborIndex == cIndex) { + continue; // Don't look at being your own neighbor } - if (CalculateDistance(input[cIndex], input[neighborIndex]) <= m_epsilon) - { + if (CalculateDistance(input[cIndex], input[neighborIndex]) <= m_epsilon) { // nrighborIndex is in our neighborhood neighbors.push_back(neighborIndex); - if (m_clusterStates[neighborIndex] == kCorePoint) - { + if (m_clusterStates[neighborIndex] == kCorePoint) { // We are at the neighborhood of a core point, we are at least a border point m_clusterStates[cIndex] = kBorderPoint; // Take the first cluster number as you can - if (clusters[cIndex] == -1) - { + if (clusters[cIndex] == -1) { clusters[cIndex] = clusters[neighborIndex]; } } } } - if (neighbors.size() >= m_minPoints - 1) - { + if (static_cast(neighbors.size()) >= m_minPoints - 1) { m_clusterStates[cIndex] = kCorePoint; - if (clusters[cIndex] == -1) - { + if (clusters[cIndex] == -1) { // We're not in the neighborhood of other core points // So we're the core of a new cluster clusters[cIndex] = nextClusterId; @@ -148,67 +132,58 @@ void DBScan::doCluster(Array &input, Array &clusters) // Set all neighbors that came before us to be border points for (int neighborListIndex = 0; - neighborListIndex < neighbors.size() && neighbors[neighborListIndex] < cIndex; neighborListIndex++) - { - if (m_clusterStates[neighbors[neighborListIndex]] == kNoise) - { + neighborListIndex < static_cast(neighbors.size()) && + neighbors[neighborListIndex] < cIndex; neighborListIndex++) { + if (m_clusterStates[neighbors[neighborListIndex]] == kNoise) { // If it was noise, now it's a border point in our cluster m_clusterStates[neighbors[neighborListIndex]] = kBorderPoint; // Make sure everything that's in our neighborhood is our cluster id clusters[neighbors[neighborListIndex]] = clusters[cIndex]; } } - } - else - { + } else { // We are a border point, or a noise point - if (m_clusterStates[cIndex] == kUnclassified) - { + if (m_clusterStates[cIndex] == kUnclassified) { m_clusterStates[cIndex] = kNoise; clusters[cIndex] = -1; } } } - m_numClusters = nextClusterId; // Number of clusters + m_numClusters = nextClusterId; // Number of clusters } -void DBScan::doClusterAndMerge(Array &input, Array &output, BBoxMergeType type) -{ +void DBScan::doClusterAndMerge(Array& input, Array& output, BBoxMergeType type) { Array clusters(m_pointsSize, true); clusters.setSize(m_pointsSize); doCluster(input, clusters); output.setSize(m_numClusters); - // merge bboxes based on different modes - if (type == MAXIMUM) - { + // merge bboxes based on different modes (TODO: might add mininum/average in the future) + if (type == MAXIMUM) { MergeMaximumBBoxes(input, clusters, output); - } - else - { + } else { throw std::runtime_error("Unsupported bbox merge type."); } } -void DBScan::doClusterAndMerge(Array &input, Array &weights, Array &output, BBoxMergeType type) -{ +void DBScan::doClusterAndMerge(Array& input, Array& weights, + Array& output, BBoxMergeType type) { Array clusters(m_pointsSize, true); clusters.setSize(m_pointsSize); doCluster(input, clusters); output.setSize(m_numClusters); // merge type must be WEIGHTED - if (type != WEIGHTED) - { + if (type != WEIGHTED) { throw std::runtime_error("Bbox merge type must be WEIGHTED."); } MergeWeightedBBoxes(input, clusters, weights, output); } -int DBScan::getNumClusters() const -{ +int DBScan::getNumClusters() const { return m_numClusters; } -}} // namespace cvcore::tensor_ops +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/DBScan.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/DBScan.h similarity index 82% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/DBScan.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/DBScan.h index 24b4dc4..2257559 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/DBScan.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/DBScan.h @@ -15,20 +15,19 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_DBSCAN_H -#define CVCORE_DBSCAN_H +#pragma once -#include "cv/core/Array.h" -#include "cv/core/BBox.h" +#include "extensions/tensorops/core/Array.h" +#include "extensions/tensorops/core/BBox.h" -namespace cvcore { namespace tensor_ops { +namespace cvcore { +namespace tensor_ops { /** * An enum. * Enum type for BBox merge type. */ -enum BBoxMergeType -{ +enum BBoxMergeType { MAXIMUM, /**< merge by expanding bounding boxes */ WEIGHTED, /**< weighted by confidence scores. */ }; @@ -36,9 +35,8 @@ enum BBoxMergeType /** * DBScan implementation used for post-processing of object detection. */ -class DBScan -{ -public: +class DBScan { + public: /** * DBScan constructor. * @param pointsize size of the input array. @@ -52,7 +50,7 @@ class DBScan * @param input input unclustered BBoxes array. * @param clusters output array containing cluster ids. */ - void doCluster(Array &input, Array &clusters); + void doCluster(Array& input, Array& clusters); /** * Run DBScan cluster and return the cluster bboxes. @@ -60,7 +58,7 @@ class DBScan * @param output output clustered BBoxes array. * @param type bbox merge type */ - void doClusterAndMerge(Array &input, Array &output, BBoxMergeType type = MAXIMUM); + void doClusterAndMerge(Array& input, Array& output, BBoxMergeType type = MAXIMUM); /** * Run DBScan cluster and return the cluster bboxes weighted on input weights. @@ -69,7 +67,7 @@ class DBScan * @param output output clustered BBoxes array. * @param type bbox merge type */ - void doClusterAndMerge(Array &input, Array &weights, Array &output, + void doClusterAndMerge(Array& input, Array& weights, Array& output, BBoxMergeType type = WEIGHTED); /** @@ -78,7 +76,7 @@ class DBScan */ int getNumClusters() const; -private: + private: int m_pointsSize; int m_numClusters; int m_minPoints; @@ -86,6 +84,5 @@ class DBScan Array m_clusterStates; }; -}} // namespace cvcore::tensor_ops - -#endif // CVCORE_DBSCAN_H +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Errors.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Errors.cpp new file mode 100644 index 0000000..e9f41f8 --- /dev/null +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Errors.cpp @@ -0,0 +1,97 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 + +#include +#include "extensions/tensorops/core/Errors.h" + +#ifndef __cpp_lib_to_underlying +// Using a C++23 feature by hacking std +namespace std { + template + constexpr underlying_type_t to_underlying(Enum e) noexcept { + return static_cast>(e); + } +}; +#endif // __cpp_lib_to_underlying + +namespace cvcore { +namespace tensor_ops { + +namespace detail { +struct TensorOpsErrorCategory : std::error_category { + const char * name() const noexcept final { + return "cvcore-tensor-ops-error"; + } + + std::string message(int value) const final { + std::string result; + + switch (value) { + case std::to_underlying(TensorOpsErrorCode::SUCCESS): + result = "(SUCCESS) No errors detected"; + break; + case std::to_underlying(TensorOpsErrorCode::COMPUTE_ENGINE_UNSUPPORTED_BY_CONTEXT): + result = "(COMPUTE_ENGINE_UNSUPPORTED_BY_CONTEXT) The selected compute " + "engine defined by cvcore::ComputeEngine is not avaible in the " + "requested context defined by cvcore::tensor_ops::TensorBackend"; + break; + case std::to_underlying(TensorOpsErrorCode::CAMERA_DISTORTION_MODEL_UNSUPPORTED): + result = "(CAMERA_DISTORTION_MODEL_UNSUPPORTED) The selected camera " + "distortion model defined by cvcore::CameraDistortionType is " + "currently unsupported"; + break; + default: + result = "(Unrecognized Condition) Value " + std::to_string(value) + + " does not map to known error code literal " + + " defined by cvcore::tensor_ops::TensorOpsErrorCode"; + break; + } + + return result; + } + + std::error_condition default_error_condition(int code) const noexcept final { + std::error_condition result; + + switch (code) { + case std::to_underlying(TensorOpsErrorCode::SUCCESS): + result = ErrorCode::SUCCESS; + break; + case std::to_underlying(TensorOpsErrorCode::COMPUTE_ENGINE_UNSUPPORTED_BY_CONTEXT): + result = ErrorCode::INVALID_ENGINE_TYPE; + break; + case std::to_underlying(TensorOpsErrorCode::CAMERA_DISTORTION_MODEL_UNSUPPORTED): + result = ErrorCode::INVALID_ARGUMENT; + break; + default: + result = ErrorCode::NOT_IMPLEMENTED; + break; + } + + return result; + } +}; +} // namespace detail + +const detail::TensorOpsErrorCategory errorCategory{}; + +std::error_code make_error_code(TensorOpsErrorCode ec) noexcept { + return {std::to_underlying(ec), errorCategory}; +} + +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/Errors.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Errors.h similarity index 77% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/Errors.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Errors.h index 4cdc6ab..0fc6044 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/Errors.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Errors.h @@ -15,21 +15,21 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_ERRORS_H -#define CVCORE_ERRORS_H +#pragma once -#include "cv/core/CVError.h" +#include "extensions/tensorops/core/CVError.h" -namespace cvcore { namespace tensor_ops { +namespace cvcore { +namespace tensor_ops { -enum class TensorOpsErrorCode : std::int32_t -{ +enum class TensorOpsErrorCode : std::int32_t { SUCCESS = 0, COMPUTE_ENGINE_UNSUPPORTED_BY_CONTEXT, CAMERA_DISTORTION_MODEL_UNSUPPORTED }; -}} // namespace cvcore::tensor_ops +} // namespace tensor_ops +} // namespace cvcore // WARNING: Extending base C++ namespace to cover cvcore error codes namespace std { @@ -37,12 +37,12 @@ namespace std { template <> struct is_error_code_enum : true_type {}; -} // namespace std +} // namespace std -namespace cvcore { namespace tensor_ops { +namespace cvcore { +namespace tensor_ops { std::error_code make_error_code(TensorOpsErrorCode) noexcept; -}} // namespace cvcore::tensor_ops - -#endif // CVCORE_ERRORS_H +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Filters.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Filters.cpp new file mode 100644 index 0000000..87242be --- /dev/null +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Filters.cpp @@ -0,0 +1,154 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2020-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +#include "extensions/tensorops/core/Filters.h" +#include "extensions/tensorops/core/MathTypes.h" +#include "extensions/tensorops/core/Memory.h" +#include "extensions/tensorops/core/NppUtils.h" +#include "nppi_filtering_functions.h" + +namespace cvcore { +namespace tensor_ops { + +bool BoxFilter(Tensor& dst, const Tensor& src, const Vector2i& maskSize, + const Vector2i& anchor, cudaStream_t stream) { + if (src.isCPU() || dst.isCPU()) { + return false; + } + NppStatus status = nppiFilterBoxBorder_8u_C3R_Ctx( + static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + {static_cast(src.getWidth()), + static_cast(src.getHeight())}, {0, 0}, + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + {static_cast(src.getWidth()), static_cast(src.getHeight())}, + {maskSize.x, maskSize.y}, {anchor.x, anchor.y}, + NPP_BORDER_REPLICATE, // Only Npp Replicate is supported!!! + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +bool BoxFilter(Tensor& dst, const Tensor& src, const Vector2i& maskSize, + const Vector2i& anchor, cudaStream_t stream) { + if (src.isCPU() || dst.isCPU()) { + return false; + } + NppStatus status = nppiFilterBoxBorder_8u_C1R_Ctx( + static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + {static_cast(src.getWidth()), + static_cast(src.getHeight())}, {0, 0}, + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + {static_cast(src.getWidth()), + static_cast(src.getHeight())}, {maskSize.x, maskSize.y}, + {anchor.x, anchor.y}, NPP_BORDER_REPLICATE, GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +bool BoxFilter(Tensor& dst, const Tensor& src, const Vector2i& maskSize, + const Vector2i& anchor, cudaStream_t stream) { + if (src.isCPU() || dst.isCPU()) { + return false; + } + NppStatus status = nppiFilterBoxBorder_16u_C1R_Ctx( + static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + {static_cast(src.getWidth()), static_cast(src.getHeight())}, {0, 0}, + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + {static_cast(src.getWidth()), + static_cast(src.getHeight())}, {maskSize.x, maskSize.y}, + {anchor.x, anchor.y}, NPP_BORDER_REPLICATE, GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +bool BoxFilter(Tensor& dst, const Tensor& src, + const Vector2i& maskSize, const Vector2i& anchor, cudaStream_t stream) { + if (src.isCPU() || dst.isCPU()) { + return false; + } + NppStatus status = nppiFilterBoxBorder_16u_C3R_Ctx( + static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + {static_cast(src.getWidth()), static_cast(src.getHeight())}, {0, 0}, + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + {static_cast(src.getWidth()), + static_cast(src.getHeight())}, {maskSize.x, maskSize.y}, + {anchor.x, anchor.y}, NPP_BORDER_REPLICATE, GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +bool BoxFilter(Tensor& dst, const Tensor& src, const Vector2i& maskSize, + const Vector2i& anchor, cudaStream_t stream) { + if (src.isCPU() || dst.isCPU()) { + return false; + } + NppStatus status = nppiFilterBoxBorder_32f_C3R_Ctx( + static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + {static_cast(src.getWidth()), static_cast(src.getHeight())}, {0, 0}, + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + {static_cast(src.getWidth()), + static_cast(src.getHeight())}, {maskSize.x, maskSize.y}, + {anchor.x, anchor.y}, NPP_BORDER_REPLICATE, GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +bool BoxFilter(Tensor& dst, const Tensor& src, const Vector2i& maskSize, + const Vector2i& anchor, cudaStream_t stream) { + if (src.isCPU() || dst.isCPU()) { + return false; + } + NppStatus status = nppiFilterBoxBorder_32f_C1R_Ctx( + static_cast(src.getData()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + {static_cast(src.getWidth()), static_cast(src.getHeight())}, {0, 0}, + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + {static_cast(src.getWidth()), + static_cast(src.getHeight())}, {maskSize.x, maskSize.y}, + {anchor.x, anchor.y}, NPP_BORDER_REPLICATE, GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/Filters.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Filters.h similarity index 74% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/Filters.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Filters.h index f764b0b..1276075 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/Filters.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Filters.h @@ -15,15 +15,13 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_FILTERS_H -#define CVCORE_FILTERS_H - -#include "cv/core/Tensor.h" -#include "cv/core/MathTypes.h" +#pragma once #include - -namespace cvcore { namespace tensor_ops { +#include "extensions/tensorops/core/MathTypes.h" +#include "extensions/tensorops/core/Tensor.h" +namespace cvcore { +namespace tensor_ops { /** * Box type filtering for three channel HWC format uint_8 type Tensor. @@ -36,8 +34,8 @@ namespace cvcore { namespace tensor_ops { * @param stream specified cuda stream. */ -void BoxFilter(Tensor &dst, const Tensor &src, const Vector2i &maskSize, - const Vector2i &anchor, cudaStream_t stream = 0); +bool BoxFilter(Tensor& dst, const Tensor& src, const Vector2i& maskSize, + const Vector2i& anchor, cudaStream_t stream = 0); /** * Box type filtering for three channel HWC format uint_16 type Tensor. * @param dst destination tensor. @@ -48,8 +46,8 @@ void BoxFilter(Tensor &dst, const Tensor &src, const V * {mask size/2, mask size/2} mask aligns with center pixel index. * @param stream specified cuda stream. */ -void BoxFilter(Tensor &dst, const Tensor &src, const Vector2i &maskSize, - const Vector2i &anchor, cudaStream_t stream = 0); +bool BoxFilter(Tensor& dst, const Tensor& src, const Vector2i& maskSize, + const Vector2i& anchor, cudaStream_t stream = 0); /** * Box type filtering for three channel HWC format float type Tensor. * @param dst destination tensor. @@ -60,8 +58,8 @@ void BoxFilter(Tensor &dst, const Tensor &src, const * {mask size/2, mask size/2} mask aligns with center pixel index. * @param stream specified cuda stream. */ -void BoxFilter(Tensor &dst, const Tensor &src, const Vector2i &maskSize, - const Vector2i &anchor, cudaStream_t stream = 0); +bool BoxFilter(Tensor& dst, const Tensor& src, const Vector2i& maskSize, + const Vector2i& anchor, cudaStream_t stream = 0); /** * Box type filtering for one channel HWC format uint_8 type Tensor. @@ -73,8 +71,8 @@ void BoxFilter(Tensor &dst, const Tensor &src, const * {mask size/2, mask size/2} mask aligns with center pixel index. * @param stream specified cuda stream. */ -void BoxFilter(Tensor &dst, const Tensor &src, const Vector2i &maskSize, - const Vector2i &anchor, cudaStream_t stream = 0); +bool BoxFilter(Tensor& dst, const Tensor& src, const Vector2i& maskSize, + const Vector2i& anchor, cudaStream_t stream = 0); /** * Box type filtering for one channel HWC format uint_16 type Tensor. * @param dst destination tensor. @@ -85,8 +83,8 @@ void BoxFilter(Tensor &dst, const Tensor &src, const V * {mask size/2, mask size/2} mask aligns with center pixel index. * @param stream specified cuda stream. */ -void BoxFilter(Tensor &dst, const Tensor &src, const Vector2i &maskSize, - const Vector2i &anchor, cudaStream_t stream = 0); +bool BoxFilter(Tensor& dst, const Tensor& src, const Vector2i& maskSize, + const Vector2i& anchor, cudaStream_t stream = 0); /** * Box type filtering for one channel HWC format float type Tensor. * @param dst destination tensor. @@ -97,9 +95,8 @@ void BoxFilter(Tensor &dst, const Tensor &src, const * {mask size/2, mask size/2} mask aligns with center pixel index. * @param stream specified cuda stream. */ -void BoxFilter(Tensor &dst, const Tensor &src, const Vector2i &maskSize, - const Vector2i &anchor, cudaStream_t stream = 0); - -}} // namespace cvcore::tensor_ops +bool BoxFilter(Tensor& dst, const Tensor& src, const Vector2i& maskSize, + const Vector2i& anchor, cudaStream_t stream = 0); -#endif // CVCORE_FILTERS_H +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/FusedOperations.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/FusedOperations.cpp similarity index 59% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/FusedOperations.cpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/FusedOperations.cpp index 4ae0c3b..8f648b4 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/FusedOperations.cpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/FusedOperations.cpp @@ -15,21 +15,21 @@ // // SPDX-License-Identifier: Apache-2.0 -#include "NppUtils.h" - -#include "cv/tensor_ops/ImageUtils.h" - -#include "cv/core/Memory.h" - #include #include +#include #include -namespace cvcore { namespace tensor_ops { +#include "extensions/tensorops/core/ImageUtils.h" +#include "extensions/tensorops/core/Memory.h" +#include "extensions/tensorops/core/NppUtils.h" + +namespace cvcore { +namespace tensor_ops { template -struct ImageToNormalizedPlanarTensorOperator::ImageToNormalizedPlanarTensorOperatorImpl -{ +struct ImageToNormalizedPlanarTensorOperator:: + ImageToNormalizedPlanarTensorOperatorImpl { int m_width; int m_height; int m_depth; @@ -40,8 +40,7 @@ struct ImageToNormalizedPlanarTensorOperator::ImageToNorm ImageToNormalizedPlanarTensorOperatorImpl(int width, int height) : m_width(width) , m_height(height) - , m_depth(1) - { + , m_depth(1) { m_resizedTensor.reset(new Tensor(width, height, false)); m_normalizedTensor.reset(new Tensor(width, height, false)); } @@ -50,23 +49,22 @@ struct ImageToNormalizedPlanarTensorOperator::ImageToNorm ImageToNormalizedPlanarTensorOperatorImpl(int width, int height, int depth) : m_width(width) , m_height(height) - , m_depth(depth) - { + , m_depth(depth) { m_resizedTensor.reset(new Tensor(width, height, depth, false)); m_normalizedTensor.reset(new Tensor(width, height, depth, false)); } template::type * = nullptr> - void execute(Tensor &dst, const Tensor &src, const float scale[3], - const float offset[3], bool swapRB, bool keep_aspect_ratio, cudaStream_t stream) - { + void execute(Tensor& dst, const Tensor& src, + const float scale[3], const float offset[3], bool swapRB, + bool keep_aspect_ratio, cudaStream_t stream) { // src and dst must be GPU tensors assert(!src.isCPU() && !dst.isCPU()); // dst image width/height must match width/height of class - if ((dst.getWidth() != m_width) || (dst.getHeight() != m_height)) - { + if ((static_cast(dst.getWidth()) != m_width) || + (static_cast(dst.getHeight()) != m_height)) { throw std::runtime_error("invalid input width/height"); } @@ -74,8 +72,7 @@ struct ImageToNormalizedPlanarTensorOperator::ImageToNorm Resize(*m_resizedTensor, src, keep_aspect_ratio, INTERP_LINEAR, stream); // swap channels if needed - if (swapRB) - { + if (swapRB) { ConvertColorFormat(*m_resizedTensor, *m_resizedTensor, BGR2RGB, stream); } @@ -88,35 +85,35 @@ struct ImageToNormalizedPlanarTensorOperator::ImageToNorm template::type * = nullptr> - void execute(Tensor &dst, const Tensor &src, const float scale[3], - const float offset[3], bool swapRB, bool keep_aspect_ratio, cudaStream_t stream) - { + void execute(Tensor& dst, const Tensor& src, + const float scale[3], const float offset[3], bool swapRB, + bool keep_aspect_ratio, cudaStream_t stream) { // src and dst must be GPU tensors assert(!src.isCPU() && !dst.isCPU()); // dst image width/height must match width/height of class - if ((dst.getWidth() != m_width) || (dst.getHeight() != m_height)) - { + if ((static_cast(dst.getWidth()) != m_width) || + (static_cast(dst.getHeight()) != m_height)) { throw std::runtime_error("invalid input width/height"); } // dst image depth must be equal to src image depth and no bigger than m_depth - if ((dst.getDepth() != src.getDepth()) || (dst.getDepth() > m_depth)) - { + if ((static_cast(dst.getDepth()) != static_cast(src.getDepth())) || + (static_cast(dst.getDepth()) > static_cast(m_depth))) { throw std::runtime_error("invalid input depth"); } // wrap the batch tensor with non-owning tensor - Tensor resizedTensor(m_width, m_height, dst.getDepth(), m_resizedTensor->getData(), false); - Tensor normalizedTensor(m_width, m_height, dst.getDepth(), m_normalizedTensor->getData(), - false); + Tensor resizedTensor(m_width, m_height, dst.getDepth(), + m_resizedTensor->getData(), false); + Tensor normalizedTensor(m_width, m_height, dst.getDepth(), + m_normalizedTensor->getData(), false); // first do the resizing Resize(resizedTensor, src, keep_aspect_ratio, INTERP_LINEAR, stream); // swap channels if needed - if (swapRB) - { + if (swapRB) { ConvertColorFormat(resizedTensor, resizedTensor, BGR2RGB, stream); } @@ -128,16 +125,15 @@ struct ImageToNormalizedPlanarTensorOperator::ImageToNorm } template::type * = nullptr> - void execute(Tensor &dst, const Tensor &src, float scale, float offset, - bool keep_aspect_ratio, cudaStream_t stream) - { + typename std::enable_if::type * = nullptr> + void execute(Tensor& dst, const Tensor& src, + float scale, float offset, bool keep_aspect_ratio, cudaStream_t stream) { // src and dst must be GPU tensors assert(!src.isCPU() && !dst.isCPU()); // dst image width/height must match width/height of class - if ((dst.getWidth() != m_width) || (dst.getHeight() != m_height)) - { + if ((static_cast(dst.getWidth()) != m_width) || + (static_cast(dst.getHeight()) != m_height)) { throw std::runtime_error("invalid input width/height"); } @@ -151,111 +147,115 @@ struct ImageToNormalizedPlanarTensorOperator::ImageToNorm template::type * = nullptr> - void execute(Tensor &dst, const Tensor &src, float scale, float offset, - bool keep_aspect_ratio, cudaStream_t stream) - { + void execute(Tensor& dst, const Tensor& src, + float scale, float offset, bool keep_aspect_ratio, cudaStream_t stream) { // src and dst must be GPU tensors assert(!src.isCPU() && !dst.isCPU()); // dst image width/height must match width/height of class - if ((dst.getWidth() != m_width) || (dst.getHeight() != m_height)) - { + if ((static_cast(dst.getWidth()) != m_width) || + (static_cast(dst.getHeight()) != m_height)) { throw std::runtime_error("invalid input width/height"); } // dst image depth must be equal to src image depth and no bigger than m_depth - if ((dst.getDepth() != src.getDepth()) || (dst.getDepth() > m_depth)) - { + if ((static_cast(dst.getDepth()) != static_cast(src.getDepth())) || + (static_cast(dst.getDepth()) > m_depth)) { throw std::runtime_error("invalid input depth"); } // wrap the batch tensor with non-owning tensor - Tensor resizedTensor(m_width, m_height, dst.getDepth(), m_resizedTensor->getData(), false); + Tensor resizedTensor(m_width, m_height, dst.getDepth(), + m_resizedTensor->getData(), false); // first do the resizing Resize(resizedTensor, src, keep_aspect_ratio, INTERP_LINEAR, stream); // do the normalization and map to destination tensor directly - Tensor output(m_width, m_height, dst.getDepth(), dst.getData(), false); + Tensor output(m_width, m_height, dst.getDepth(), + dst.getData(), false); Normalize(output, resizedTensor, scale, offset, stream); } }; template template::type *> -ImageToNormalizedPlanarTensorOperator::ImageToNormalizedPlanarTensorOperator(int width, - int height) - : m_pImpl(new ImageToNormalizedPlanarTensorOperatorImpl(width, height)) -{ +ImageToNormalizedPlanarTensorOperator + ::ImageToNormalizedPlanarTensorOperator(int width, int height) + : m_pImpl(new ImageToNormalizedPlanarTensorOperatorImpl(width, height)) { static_assert(TL_IN == HWC && TL_OUT == CHW, "Tensor Layout is different"); static_assert(CC == C1 || CC == C3, "Channel count is different"); } template template::type *> -ImageToNormalizedPlanarTensorOperator::ImageToNormalizedPlanarTensorOperator(int width, - int height, - int depth) - : m_pImpl(new ImageToNormalizedPlanarTensorOperatorImpl(width, height, depth)) -{ +ImageToNormalizedPlanarTensorOperator:: + ImageToNormalizedPlanarTensorOperator(int width, int height, int depth) + : m_pImpl(new ImageToNormalizedPlanarTensorOperatorImpl(width, height, depth)) { static_assert(TL_IN == NHWC && TL_OUT == NCHW, "Tensor Layout is different"); static_assert(CC == C1 || CC == C3, "Channel count is different"); } template -ImageToNormalizedPlanarTensorOperator::~ImageToNormalizedPlanarTensorOperator() -{ +ImageToNormalizedPlanarTensorOperator:: + ~ImageToNormalizedPlanarTensorOperator() { } template template::type *> void ImageToNormalizedPlanarTensorOperator::operator()( - Tensor &dst, const Tensor &src, const float scale[3], const float offset[3], - bool swapRB, bool keep_aspect_ratio, cudaStream_t stream) -{ + Tensor& dst, const Tensor& src, + const float scale[3], const float offset[3], + bool swapRB, bool keep_aspect_ratio, cudaStream_t stream) { m_pImpl->execute(dst, src, scale, offset, swapRB, keep_aspect_ratio, stream); } template template::type *> -void ImageToNormalizedPlanarTensorOperator::operator()(Tensor &dst, - const Tensor &src, - float scale, float offset, - bool keep_aspect_ratio, - cudaStream_t stream) -{ +void ImageToNormalizedPlanarTensorOperator::operator()( + Tensor& dst, + const Tensor& src, + float scale, float offset, + bool keep_aspect_ratio, + cudaStream_t stream) { m_pImpl->execute(dst, src, scale, offset, keep_aspect_ratio, stream); } // explicit instantiations template class ImageToNormalizedPlanarTensorOperator; -template void ImageToNormalizedPlanarTensorOperator::operator()(Tensor &, - const Tensor &, - const float [], const float [], - bool, bool, cudaStream_t); -template ImageToNormalizedPlanarTensorOperator::ImageToNormalizedPlanarTensorOperator(int, int); +template void ImageToNormalizedPlanarTensorOperator::operator()( + Tensor& , + const Tensor& , + const float[], const float[], + bool, bool, cudaStream_t); +template ImageToNormalizedPlanarTensorOperator:: + ImageToNormalizedPlanarTensorOperator(int, int); template class ImageToNormalizedPlanarTensorOperator; -template void ImageToNormalizedPlanarTensorOperator::operator()(Tensor &, - const Tensor &, - float, float, bool, cudaStream_t); -template ImageToNormalizedPlanarTensorOperator::ImageToNormalizedPlanarTensorOperator(int, int); +template void ImageToNormalizedPlanarTensorOperator::operator()( + Tensor&, + const Tensor&, + float, float, bool, cudaStream_t); +template ImageToNormalizedPlanarTensorOperator:: + ImageToNormalizedPlanarTensorOperator(int, int); template class ImageToNormalizedPlanarTensorOperator; -template void ImageToNormalizedPlanarTensorOperator::operator()(Tensor &, - const Tensor &, - const float [], const float [], - bool, bool, cudaStream_t); -template ImageToNormalizedPlanarTensorOperator::ImageToNormalizedPlanarTensorOperator(int, - int, - int); +template void ImageToNormalizedPlanarTensorOperator::operator()( + Tensor&, + const Tensor&, + const float[], const float[], + bool, bool, cudaStream_t); +template ImageToNormalizedPlanarTensorOperator:: + ImageToNormalizedPlanarTensorOperator(int, int, int); template class ImageToNormalizedPlanarTensorOperator; -template void ImageToNormalizedPlanarTensorOperator::operator()(Tensor &, - const Tensor &, - float, float, bool, - cudaStream_t); -template ImageToNormalizedPlanarTensorOperator::ImageToNormalizedPlanarTensorOperator(int, - int, - int); -}} // namespace cvcore::tensor_ops +template void ImageToNormalizedPlanarTensorOperator::operator()( + Tensor&, + const Tensor&, + float, float, bool, + cudaStream_t); +template ImageToNormalizedPlanarTensorOperator:: + ImageToNormalizedPlanarTensorOperator(int, int, int); + +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/GeometryTransforms.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/GeometryTransforms.cpp new file mode 100644 index 0000000..cf78446 --- /dev/null +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/GeometryTransforms.cpp @@ -0,0 +1,969 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2020-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +#include "extensions/tensorops/core/ImageUtils.h" +#include "extensions/tensorops/core/Memory.h" +#include "extensions/tensorops/core/NppUtils.h" +#include "nppi_data_exchange_and_initialization.h" +#include "nppi_geometry_transforms.h" + +namespace cvcore { +namespace tensor_ops { +namespace { + +static NppiInterpolationMode GetNppiInterpolationMode(InterpolationType type) { + if (type == INTERP_NEAREST) { + return NPPI_INTER_NN; + } else if (type == INTERP_LINEAR) { + return NPPI_INTER_LINEAR; + } else if (type == INTERP_CUBIC_BSPLINE) { + return NPPI_INTER_CUBIC2P_BSPLINE; + } else if (type == INTERP_CUBIC_CATMULLROM) { + return NPPI_INTER_CUBIC2P_CATMULLROM; + } else { + throw std::runtime_error("invalid resizing interpolation mode"); + } +} + +static BBox GetScaledROI(int srcW, int srcH, int dstW, int dstH) { + if (srcW * dstH >= dstW * srcH) { + int bboxH = static_cast((static_cast(srcH) / srcW) * dstW); + int offsetH = (dstH - bboxH) / 2; + return {0, offsetH, dstW, offsetH + bboxH}; + } else { + int bboxW = static_cast((static_cast(srcW) / srcH) * dstH); + int offsetW = (dstW - bboxW) / 2; + return {offsetW, 0, offsetW + bboxW, dstH}; + } +} + +static bool CheckValidROI(const BBox& roi, int width, int height) { + if ((roi.xmin >= 0 && roi.xmin < roi.xmax) && + (roi.ymin >= 0 && roi.ymin < roi.ymax) && + (roi.ymax <= height) && + (roi.xmax <= width)) { + return true; + } else { + return false; + } +} + +template +bool FillBufferC1U8Impl(Tensor& dst, const Npp8u value, cudaStream_t stream) { + if (dst.isCPU()) { + return false; + } + NppStatus status = nppiSet_8u_C1R_Ctx(value, static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + {static_cast(dst.getWidth()), static_cast(dst.getHeight())}, + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +template +bool FillBufferC1U16Impl(Tensor& dst, const Npp16u value, cudaStream_t stream) { + if (dst.isCPU()) { + return false; + } + NppStatus status = nppiSet_16u_C1R_Ctx(value, static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + {static_cast(dst.getWidth()), static_cast(dst.getHeight())}, + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +template +bool FillBufferC1F32Impl(Tensor& dst, const Npp32f value, cudaStream_t stream) { + if (dst.isCPU()) { + return false; + } + NppStatus status = nppiSet_32f_C1R_Ctx(value, static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + {static_cast(dst.getWidth()), static_cast(dst.getHeight())}, + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +static bool FillBuffer(Tensor& dst, const Npp8u value, cudaStream_t stream = 0) { + return FillBufferC1U8Impl(dst, value, stream); +} + +static bool FillBuffer(Tensor& dst, const Npp16u value, cudaStream_t stream = 0) { + return FillBufferC1U16Impl(dst, value, stream); +} + +static bool FillBuffer(Tensor& dst, const Npp32f value, cudaStream_t stream = 0) { + return FillBufferC1F32Impl(dst, value, stream); +} + +static bool FillBuffer(Tensor& dst, const Npp8u value, cudaStream_t stream = 0) { + return FillBufferC1U8Impl(dst, value, stream); +} + +static bool FillBuffer(Tensor& dst, const Npp16u value, cudaStream_t stream = 0) { + return FillBufferC1U16Impl(dst, value, stream); +} + +static bool FillBuffer(Tensor& dst, const Npp32f value, cudaStream_t stream = 0) { + return FillBufferC1F32Impl(dst, value, stream); +} + +static bool FillBuffer(Tensor& dst, const Npp8u value, cudaStream_t stream = 0) { + if (dst.isCPU()) { + return false; + } + const Npp8u padding[3] = {value, value, value}; + NppStatus status = nppiSet_8u_C3R_Ctx(padding, static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + {static_cast(dst.getWidth()), static_cast(dst.getHeight())}, + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +static bool FillBuffer(Tensor& dst, const Npp16u value, cudaStream_t stream = 0) { + if (dst.isCPU()) { + return false; + } + const Npp16u padding[3] = {value, value, value}; + NppStatus status = nppiSet_16u_C3R_Ctx(padding, static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + {static_cast(dst.getWidth()), static_cast(dst.getHeight())}, + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +static bool FillBuffer(Tensor& dst, const Npp32f value, cudaStream_t stream = 0) { + if (dst.isCPU()) { + return false; + } + const Npp32f padding[3] = {value, value, value}; + NppStatus status = nppiSet_32f_C3R_Ctx(padding, static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + {static_cast(dst.getWidth()), static_cast(dst.getHeight())}, + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +template +bool CropAndResizeC1U8Impl(Tensor& dst, const Tensor& src, + const BBox& dstROI, const BBox& srcROI, InterpolationType type, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if (!CheckValidROI(dstROI, dst.getWidth(), dst.getHeight())) { + return false; + } + if (!CheckValidROI(srcROI, src.getWidth(), src.getHeight())) { + return false; + } + + NppStatus status = nppiResizeSqrPixel_8u_C1R_Ctx( + static_cast(src.getData() + srcROI.ymin * + src.getStride(TensorDimension::HEIGHT) + srcROI.xmin), + {srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + {0, 0, srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, + static_cast(dst.getData() + dstROI.ymin * + dst.getStride(TensorDimension::HEIGHT) + dstROI.xmin), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + {0, 0, dstROI.xmax - dstROI.xmin, dstROI.ymax - dstROI.ymin}, + static_cast(dstROI.xmax - dstROI.xmin) / + static_cast(srcROI.xmax - srcROI.xmin), + static_cast(dstROI.ymax - dstROI.ymin) / + static_cast(srcROI.ymax - srcROI.ymin), + 0.0, 0.0, GetNppiInterpolationMode(type), + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +template +bool CropAndResizeC1U16Impl(Tensor& dst, const Tensor& src, + const BBox& dstROI, const BBox& srcROI, InterpolationType type, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if (!CheckValidROI(dstROI, dst.getWidth(), dst.getHeight())) { + return false; + } + if (!CheckValidROI(srcROI, src.getWidth(), src.getHeight())) { + return false; + } + + NppStatus status = nppiResizeSqrPixel_16u_C1R_Ctx( + static_cast(src.getData() + srcROI.ymin * + src.getStride(TensorDimension::HEIGHT) + srcROI.xmin), + {srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + {0, 0, srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, + static_cast(dst.getData() + dstROI.ymin * + dst.getStride(TensorDimension::HEIGHT) + dstROI.xmin), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + {0, 0, dstROI.xmax - dstROI.xmin, dstROI.ymax - dstROI.ymin}, + static_cast(dstROI.xmax - dstROI.xmin) / + static_cast(srcROI.xmax - srcROI.xmin), + static_cast(dstROI.ymax - dstROI.ymin) / + static_cast(srcROI.ymax - srcROI.ymin), + 0.0, 0.0, GetNppiInterpolationMode(type), + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +template +bool CropAndResizeC1F32Impl(Tensor& dst, const Tensor& src, + const BBox& dstROI, const BBox& srcROI, InterpolationType type, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if (!CheckValidROI(dstROI, dst.getWidth(), dst.getHeight())) { + return false; + } + if (!CheckValidROI(srcROI, src.getWidth(), src.getHeight())) { + return false; + } + + NppStatus status = nppiResizeSqrPixel_32f_C1R_Ctx( + static_cast(src.getData() + srcROI.ymin * + src.getStride(TensorDimension::HEIGHT) + srcROI.xmin), + {srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + {0, 0, srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, + static_cast(dst.getData() + dstROI.ymin * + dst.getStride(TensorDimension::HEIGHT) + dstROI.xmin), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + {0, 0, dstROI.xmax - dstROI.xmin, dstROI.ymax - dstROI.ymin}, + static_cast(dstROI.xmax - dstROI.xmin) / + static_cast(srcROI.xmax - srcROI.xmin), + static_cast(dstROI.ymax - dstROI.ymin) / + static_cast(srcROI.ymax - srcROI.ymin), + 0.0, 0.0, GetNppiInterpolationMode(type), + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +template +bool ResizeImpl(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio, InterpolationType type, + cudaStream_t stream) { + bool ret = true; + const BBox dstROI = keep_aspect_ratio + ? GetScaledROI(src.getWidth(), src.getHeight(), + dst.getWidth(), dst.getHeight()) + : BBox{0, 0, static_cast(dst.getWidth()), + static_cast(dst.getHeight())}; + if (keep_aspect_ratio) { + ret = FillBuffer(dst, 0, stream); + if (!ret) { + return false; + } + } + ret = CropAndResize(dst, src, dstROI, {0, 0, static_cast(src.getWidth()), + static_cast(src.getHeight())}, type, stream); + return ret; +} + +template +bool CropC1U8Impl(Tensor& dst, const Tensor& src, + const BBox& srcROI, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if (!CheckValidROI(srcROI, src.getWidth(), src.getHeight())) { + return false; + } + if ((srcROI.xmax - srcROI.xmin) != static_cast(dst.getWidth()) || + (srcROI.ymax - srcROI.ymin) != static_cast(dst.getHeight())) { + return false; + } + + NppStatus status = nppiCopy_8u_C1R_Ctx( + static_cast(src.getData() + srcROI.ymin * + src.getStride(TensorDimension::HEIGHT) + srcROI.xmin), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + {srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +template +bool CropC1U16Impl(Tensor& dst, const Tensor& src, + const BBox& srcROI, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if (!CheckValidROI(srcROI, src.getWidth(), src.getHeight())) { + return false; + } + if ((srcROI.xmax - srcROI.xmin) != static_cast(dst.getWidth()) || + (srcROI.ymax - srcROI.ymin) != static_cast(dst.getHeight())) { + return false; + } + + NppStatus status = nppiCopy_16u_C1R_Ctx( + static_cast(src.getData() + srcROI.ymin * + src.getStride(TensorDimension::HEIGHT) + srcROI.xmin), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + {srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +template +bool CropC1F32Impl(Tensor& dst, const Tensor& src, + const BBox& srcROI, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if (!CheckValidROI(srcROI, src.getWidth(), src.getHeight())) { + return false; + } + if ((srcROI.xmax - srcROI.xmin) != static_cast(dst.getWidth()) || + (srcROI.ymax - srcROI.ymin) != static_cast(dst.getHeight())) { + return false; + } + + NppStatus status = nppiCopy_32f_C1R_Ctx( + static_cast(src.getData() + srcROI.ymin * + src.getStride(TensorDimension::HEIGHT) + srcROI.xmin), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + {srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +template +bool ResizeBatch(Tensor& dst, Tensor& src, + bool keep_aspect_ratio, InterpolationType type, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if (src.getDepth() != dst.getDepth()) { + return false; + } + + for (size_t i = 0; i < src.getDepth(); i++) { + size_t offsetSrc = i * src.getStride(TensorDimension::DEPTH); + size_t offsetDst = i * dst.getStride(TensorDimension::DEPTH); + Tensor srcTmp(src.getWidth(), src.getHeight(), + src.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), + src.getData() + offsetSrc, false); + Tensor dstTmp(dst.getWidth(), dst.getHeight(), + dst.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), + dst.getData() + offsetDst, false); + bool ret = Resize(dstTmp, srcTmp, keep_aspect_ratio, type, stream); + if (!ret) { + return false; + } + } + return true; +} + +} // anonymous namespace + +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio, InterpolationType type, cudaStream_t stream) { + return ResizeImpl(dst, src, keep_aspect_ratio, type, stream); +} + +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio, InterpolationType type, cudaStream_t stream) { + return ResizeBatch(dst, const_cast &>(src), + keep_aspect_ratio, type, stream); +} + +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& dstROI, const BBox& srcROI, InterpolationType type, cudaStream_t stream) { + return CropAndResizeC1U8Impl(dst, src, dstROI, srcROI, type, stream); +} + +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& srcROI, InterpolationType type, cudaStream_t stream) { + return CropAndResize(dst, src, {0, 0, static_cast(dst.getWidth()), + static_cast(dst.getHeight())}, srcROI, type, stream); +} + +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio, InterpolationType type, cudaStream_t stream) { + return ResizeImpl(dst, src, keep_aspect_ratio, type, stream); +} + +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio, InterpolationType type, cudaStream_t stream) { + return ResizeBatch(dst, const_cast &>(src), + keep_aspect_ratio, type, stream); +} + +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& dstROI, const BBox& srcROI, InterpolationType type, cudaStream_t stream) { + return CropAndResizeC1U16Impl(dst, src, dstROI, srcROI, type, stream); +} + +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& srcROI, InterpolationType type, cudaStream_t stream) { + return CropAndResize(dst, src, {0, 0, static_cast(dst.getWidth()), + static_cast(dst.getHeight())}, srcROI, type, stream); +} + +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio, InterpolationType type, cudaStream_t stream) { + return ResizeImpl(dst, src, keep_aspect_ratio, type, stream); +} + +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio, InterpolationType type, cudaStream_t stream) { + return ResizeBatch(dst, const_cast &>(src), + keep_aspect_ratio, type, stream); +} + +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& dstROI, const BBox& srcROI, InterpolationType type, cudaStream_t stream) { + return CropAndResizeC1F32Impl(dst, src, dstROI, srcROI, type, stream); +} + +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& srcROI, InterpolationType type, cudaStream_t stream) { + return CropAndResize(dst, src, {0, 0, static_cast(dst.getWidth()), + static_cast(dst.getHeight())}, srcROI, type, stream); +} + +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio, InterpolationType type, cudaStream_t stream) { + return ResizeImpl(dst, src, keep_aspect_ratio, type, stream); +} + +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& dstROI, const BBox& srcROI, InterpolationType type, cudaStream_t stream) { + return CropAndResizeC1U8Impl(dst, src, dstROI, srcROI, type, stream); +} + +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& srcROI, InterpolationType type, cudaStream_t stream) { + return CropAndResize(dst, src, {0, 0, static_cast(dst.getWidth()), + static_cast(dst.getHeight())}, srcROI, type, stream); +} + +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio, InterpolationType type, cudaStream_t stream) { + return ResizeImpl(dst, src, keep_aspect_ratio, type, stream); +} + +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& dstROI, const BBox& srcROI, InterpolationType type, cudaStream_t stream) { + return CropAndResizeC1U16Impl(dst, src, dstROI, srcROI, type, stream); +} + +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& srcROI, InterpolationType type, cudaStream_t stream) { + return CropAndResize(dst, src, {0, 0, static_cast(dst.getWidth()), + static_cast(dst.getHeight())}, srcROI, type, stream); +} + +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio, InterpolationType type, cudaStream_t stream) { + return ResizeImpl(dst, src, keep_aspect_ratio, type, stream); +} + +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& dstROI, const BBox& srcROI, InterpolationType type, cudaStream_t stream) { + return CropAndResizeC1F32Impl(dst, src, dstROI, srcROI, type, stream); +} + +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& srcROI, InterpolationType type, cudaStream_t stream) { + return CropAndResize(dst, src, {0, 0, static_cast(dst.getWidth()), + static_cast(dst.getHeight())}, srcROI, type, stream); +} + +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio, InterpolationType type, cudaStream_t stream) { + return ResizeImpl(dst, src, keep_aspect_ratio, type, stream); +} + +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio, InterpolationType type, cudaStream_t stream) { + return ResizeBatch(dst, const_cast &>(src), + keep_aspect_ratio, type, stream); +} + +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& dstROI, const BBox& srcROI, InterpolationType type, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if (!CheckValidROI(dstROI, dst.getWidth(), dst.getHeight())) { + return false; + } + if (!CheckValidROI(srcROI, src.getWidth(), src.getHeight())) { + return false; + } + + NppStatus status = nppiResizeSqrPixel_8u_C3R_Ctx( + static_cast(src.getData() + srcROI.ymin * + src.getStride(TensorDimension::HEIGHT) + + srcROI.xmin * src.getChannelCount()), + {srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + {0, 0, srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, + static_cast(dst.getData() + dstROI.ymin * + dst.getStride(TensorDimension::HEIGHT) + + dstROI.xmin * dst.getChannelCount()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + {0, 0, dstROI.xmax - dstROI.xmin, dstROI.ymax - dstROI.ymin}, + static_cast(dstROI.xmax - dstROI.xmin) / + static_cast(srcROI.xmax - srcROI.xmin), + static_cast(dstROI.ymax - dstROI.ymin) / + static_cast(srcROI.ymax - srcROI.ymin), + 0.0, 0.0, GetNppiInterpolationMode(type), + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& srcROI, InterpolationType type, cudaStream_t stream) { + return CropAndResize(dst, src, {0, 0, static_cast(dst.getWidth()), + static_cast(dst.getHeight())}, srcROI, type, stream); +} + +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio, InterpolationType type, cudaStream_t stream) { + return ResizeImpl(dst, src, keep_aspect_ratio, type, stream); +} + +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio, InterpolationType type, cudaStream_t stream) { + return ResizeBatch(dst, const_cast &>(src), + keep_aspect_ratio, type, stream); +} + +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& dstROI, const BBox& srcROI, InterpolationType type, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if (!CheckValidROI(dstROI, dst.getWidth(), dst.getHeight())) { + return false; + } + if (!CheckValidROI(srcROI, src.getWidth(), src.getHeight())) { + return false; + } + + NppStatus status = nppiResizeSqrPixel_16u_C3R_Ctx( + static_cast(src.getData() + srcROI.ymin * + src.getStride(TensorDimension::HEIGHT) + + srcROI.xmin * src.getChannelCount()), + {srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + {0, 0, srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, + static_cast(dst.getData() + dstROI.ymin * + dst.getStride(TensorDimension::HEIGHT) + + dstROI.xmin * dst.getChannelCount()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + {0, 0, dstROI.xmax - dstROI.xmin, dstROI.ymax - dstROI.ymin}, + static_cast(dstROI.xmax - dstROI.xmin) / + static_cast(srcROI.xmax - srcROI.xmin), + static_cast(dstROI.ymax - dstROI.ymin) / + static_cast(srcROI.ymax - srcROI.ymin), + 0.0, 0.0, GetNppiInterpolationMode(type), + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& srcROI, InterpolationType type, cudaStream_t stream) { + return CropAndResize(dst, src, {0, 0, static_cast(dst.getWidth()), + static_cast(dst.getHeight())}, srcROI, type, stream); +} + +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio, InterpolationType type, cudaStream_t stream) { + return ResizeImpl(dst, src, keep_aspect_ratio, type, stream); +} + +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio, InterpolationType type, cudaStream_t stream) { + return ResizeBatch(dst, const_cast &>(src), + keep_aspect_ratio, type, stream); +} + +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& dstROI, const BBox& srcROI, InterpolationType type, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if (!CheckValidROI(dstROI, dst.getWidth(), dst.getHeight())) { + return false; + } + if (!CheckValidROI(srcROI, src.getWidth(), src.getHeight())) { + return false; + } + + NppStatus status = nppiResizeSqrPixel_32f_C3R_Ctx( + static_cast(src.getData() + srcROI.ymin * + src.getStride(TensorDimension::HEIGHT) + + srcROI.xmin * src.getChannelCount()), + {srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + {0, 0, srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, + static_cast(dst.getData() + dstROI.ymin * + dst.getStride(TensorDimension::HEIGHT) + + dstROI.xmin * dst.getChannelCount()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + {0, 0, dstROI.xmax - dstROI.xmin, dstROI.ymax - dstROI.ymin}, + static_cast(dstROI.xmax - dstROI.xmin) / + static_cast(srcROI.xmax - srcROI.xmin), + static_cast(dstROI.ymax - dstROI.ymin) / + static_cast(srcROI.ymax - srcROI.ymin), + 0.0, 0.0, GetNppiInterpolationMode(type), + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& srcROI, InterpolationType type, cudaStream_t stream) { + return CropAndResize(dst, src, {0, 0, static_cast(dst.getWidth()), + static_cast(dst.getHeight())}, srcROI, type, stream); +} + +bool Crop(Tensor& dst, const Tensor& src, + const BBox& srcROI, cudaStream_t stream) { + return CropC1U8Impl(dst, src, srcROI, stream); +} + +bool Crop(Tensor& dst, const Tensor& src, + const BBox& srcROI, cudaStream_t stream) { + return CropC1U16Impl(dst, src, srcROI, stream); +} + +bool Crop(Tensor& dst, const Tensor& src, + const BBox& srcROI, cudaStream_t stream) { + return CropC1F32Impl(dst, src, srcROI, stream); +} + +bool Crop(Tensor& dst, const Tensor& src, + const BBox& srcROI, cudaStream_t stream) { + return CropC1U8Impl(dst, src, srcROI, stream); +} + +bool Crop(Tensor& dst, const Tensor& src, + const BBox& srcROI, cudaStream_t stream) { + return CropC1U16Impl(dst, src, srcROI, stream); +} + +bool Crop(Tensor& dst, const Tensor& src, + const BBox& srcROI, cudaStream_t stream) { + return CropC1F32Impl(dst, src, srcROI, stream); +} + +bool Crop(Tensor& dst, const Tensor& src, + const BBox& srcROI, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if (!CheckValidROI(srcROI, src.getWidth(), src.getHeight())) { + return false; + } + if ((srcROI.xmax - srcROI.xmin) != static_cast(dst.getWidth()) || + (srcROI.ymax - srcROI.ymin) != static_cast(dst.getHeight())) { + return false; + } + + NppStatus status = nppiCopy_8u_C3R_Ctx( + static_cast(src.getData() + srcROI.ymin * + src.getStride(TensorDimension::HEIGHT) + + srcROI.xmin * src.getChannelCount()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + {srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +bool Crop(Tensor& dst, const Tensor& src, + const BBox& srcROI, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if (!CheckValidROI(srcROI, src.getWidth(), src.getHeight())) { + return false; + } + if ((srcROI.xmax - srcROI.xmin) != static_cast(dst.getWidth()) || + (srcROI.ymax - srcROI.ymin) != static_cast(dst.getHeight())) { + return false; + } + + NppStatus status = nppiCopy_16u_C3R_Ctx( + static_cast(src.getData() + srcROI.ymin * + src.getStride(TensorDimension::HEIGHT) + + srcROI.xmin * src.getChannelCount()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + {srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +bool Crop(Tensor& dst, const Tensor& src, + const BBox& srcROI, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + if (!CheckValidROI(srcROI, src.getWidth(), src.getHeight())) { + return false; + } + if ((srcROI.xmax - srcROI.xmin) != static_cast(dst.getWidth()) || + (srcROI.ymax - srcROI.ymin) != static_cast(dst.getHeight())) { + return false; + } + + NppStatus status = nppiCopy_32f_C3R_Ctx( + static_cast(src.getData() + srcROI.ymin * + src.getStride(TensorDimension::HEIGHT) + + srcROI.xmin * src.getChannelCount()), + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + {srcROI.xmax - srcROI.xmin, srcROI.ymax - srcROI.ymin}, + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +bool WarpPerspective(Tensor& dst, const Tensor& src, + const double coeffs[3][3], InterpolationType type, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + + NppStatus status = nppiWarpPerspective_8u_C1R_Ctx( + static_cast(src.getData()), {static_cast(src.getWidth()), + static_cast(src.getHeight())}, + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + {0, 0, static_cast(src.getWidth()), static_cast(src.getHeight())}, + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + {0, 0, static_cast(dst.getWidth()), static_cast(dst.getHeight())}, coeffs, + GetNppiInterpolationMode(type), + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +bool WarpPerspective(Tensor& dst, const Tensor& src, + const double coeffs[3][3], InterpolationType type, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + + NppStatus status = nppiWarpPerspective_16u_C1R_Ctx( + static_cast(src.getData()), + {static_cast(src.getWidth()), static_cast(src.getHeight())}, + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + {0, 0, static_cast(src.getWidth()), static_cast(src.getHeight())}, + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + {0, 0, static_cast(dst.getWidth()), + static_cast(dst.getHeight())}, coeffs, GetNppiInterpolationMode(type), + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +bool WarpPerspective(Tensor& dst, const Tensor& src, + const double coeffs[3][3], InterpolationType type, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + + NppStatus status = nppiWarpPerspective_32f_C1R_Ctx( + static_cast(src.getData()), + {static_cast(src.getWidth()), static_cast(src.getHeight())}, + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + {0, 0, static_cast(src.getWidth()), static_cast(src.getHeight())}, + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + {0, 0, static_cast(dst.getWidth()), + static_cast(dst.getHeight())}, + coeffs, GetNppiInterpolationMode(type), + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +bool WarpPerspective(Tensor& dst, const Tensor& src, + const double coeffs[3][3], InterpolationType type, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + + NppStatus status = nppiWarpPerspective_8u_C3R_Ctx( + static_cast(src.getData()), {static_cast(src.getWidth()), + static_cast(src.getHeight())}, + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + {0, 0, static_cast(src.getWidth()), + static_cast(src.getHeight())}, + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp8u), + {0, 0, static_cast(dst.getWidth()), + static_cast(dst.getHeight())}, coeffs, + GetNppiInterpolationMode(type), + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +bool WarpPerspective(Tensor& dst, const Tensor& src, + const double coeffs[3][3], InterpolationType type, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + + NppStatus status = nppiWarpPerspective_16u_C3R_Ctx( + static_cast(src.getData()), + {static_cast(src.getWidth()), + static_cast(src.getHeight())}, + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + {0, 0, static_cast(src.getWidth()), + static_cast(src.getHeight())}, + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp16u), + {0, 0, static_cast(dst.getWidth()), + static_cast(dst.getHeight())}, + coeffs, GetNppiInterpolationMode(type), + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +bool WarpPerspective(Tensor& dst, const Tensor& src, + const double coeffs[3][3], InterpolationType type, cudaStream_t stream) { + // src and dst must be GPU tensors + if (src.isCPU() || dst.isCPU()) { + return false; + } + + NppStatus status = nppiWarpPerspective_32f_C3R_Ctx( + static_cast(src.getData()), + {static_cast(src.getWidth()), + static_cast(src.getHeight())}, + src.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + {0, 0, static_cast(src.getWidth()), + static_cast(src.getHeight())}, + static_cast(dst.getData()), + dst.getStride(TensorDimension::HEIGHT) * sizeof(Npp32f), + {0, 0, static_cast(dst.getWidth()), + static_cast(dst.getHeight())}, coeffs, + GetNppiInterpolationMode(type), + GetNppStreamContext(stream)); + if (status != NPP_SUCCESS) { + return false; + } + return true; +} + +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/IImageWarp.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/IImageWarp.cpp similarity index 81% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/IImageWarp.cpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/IImageWarp.cpp index c96b07e..c4144f6 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/IImageWarp.cpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/IImageWarp.cpp @@ -15,10 +15,12 @@ // // SPDX-License-Identifier: Apache-2.0 -#include "cv/tensor_ops/IImageWarp.h" +#include "extensions/tensorops/core/IImageWarp.h" -namespace cvcore { namespace tensor_ops { +namespace cvcore { +namespace tensor_ops { -IImageWarp::~IImageWarp(){} +IImageWarp::~IImageWarp() {} -}} // namespace cvcore::tensor_ops +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/IImageWarp.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/IImageWarp.h similarity index 74% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/IImageWarp.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/IImageWarp.h index 4a81e1a..ba5ab32 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/IImageWarp.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/IImageWarp.h @@ -15,17 +15,16 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_IIMAGEWARP_H -#define CVCORE_IIMAGEWARP_H +#pragma once -#include #include +#include #include -namespace cvcore { namespace tensor_ops { +namespace cvcore { +namespace tensor_ops { -struct ImageGrid -{ +struct ImageGrid { static constexpr std::size_t MAX_HORIZ_REGIONS = 4; static constexpr std::size_t MAX_VERT_REGIONS = 4; static constexpr std::size_t MIN_REGION_WIDTH = 64; @@ -39,25 +38,23 @@ struct ImageGrid std::array regionWidth; }; -class IImageWarp -{ - public: +class IImageWarp { + public: // Public Destructor virtual ~IImageWarp() = 0; - protected: + protected: // Protected Constructor(s) - IImageWarp() = default; - IImageWarp(const IImageWarp &) = default; - IImageWarp(IImageWarp &&) noexcept = default; + IImageWarp() = default; + IImageWarp(const IImageWarp&) = default; + IImageWarp(IImageWarp&&) noexcept = default; // Protected Operator(s) - IImageWarp &operator=(const IImageWarp &) = default; - IImageWarp &operator=(IImageWarp &&) noexcept = default; + IImageWarp& operator=(const IImageWarp&) = default; + IImageWarp& operator=(IImageWarp&&) noexcept = default; }; using ImageWarp = IImageWarp*; -}} // namespace cvcore::tensor_ops - -#endif // CVCORE_IIMAGEWARP_H +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/ITensorOperatorContext.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/ITensorOperatorContext.h similarity index 58% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/ITensorOperatorContext.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/ITensorOperatorContext.h index 5aa6cdb..f49fd19 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/ITensorOperatorContext.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/ITensorOperatorContext.h @@ -15,51 +15,49 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_ITENSOROPERATORCONTEXT_H -#define CVCORE_ITENSOROPERATORCONTEXT_H +#pragma once #include -#include "ITensorOperatorStream.h" -#include "cv/core/CVError.h" -#include "cv/core/ComputeEngine.h" +#include "extensions/tensorops/core/ComputeEngine.h" +#include "extensions/tensorops/core/CVError.h" +#include "extensions/tensorops/core/ITensorOperatorStream.h" -namespace cvcore { namespace tensor_ops { +namespace cvcore { +namespace tensor_ops { -enum class TensorBackend : std::uint8_t -{ +enum class TensorBackend : std::uint8_t { NPP, VPI, DALI }; -class ITensorOperatorContext -{ -public: +class ITensorOperatorContext { + public: // Public Constructor(s)/Destructor virtual ~ITensorOperatorContext() noexcept = default; // Public Accessor Method(s) - virtual std::error_code CreateStream(TensorOperatorStream &, const ComputeEngine &) = 0; - virtual std::error_code DestroyStream(TensorOperatorStream &) = 0; + virtual std::error_code CreateStream(TensorOperatorStream&, + const ComputeEngine&) = 0; + virtual std::error_code DestroyStream(TensorOperatorStream&) = 0; - virtual bool IsComputeEngineCompatible(const ComputeEngine &) const noexcept = 0; + virtual bool IsComputeEngineCompatible(const ComputeEngine&) const noexcept = 0; virtual TensorBackend Backend() const noexcept = 0; -protected: + protected: // Protected Constructor(s) ITensorOperatorContext() = default; - ITensorOperatorContext(const ITensorOperatorContext &) = default; + ITensorOperatorContext(const ITensorOperatorContext&) = default; ITensorOperatorContext(ITensorOperatorContext &&) noexcept = default; // Protected Operator(s) - ITensorOperatorContext &operator=(const ITensorOperatorContext &) = default; - ITensorOperatorContext &operator=(ITensorOperatorContext &&) noexcept = default; + ITensorOperatorContext& operator=(const ITensorOperatorContext&) = default; + ITensorOperatorContext& operator=(ITensorOperatorContext&&) noexcept = default; }; using TensorOperatorContext = ITensorOperatorContext *; -}} // namespace cvcore::tensor_ops - -#endif // CVCORE_ITENSOROPERATORCONTEXT_H +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/ITensorOperatorStream.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/ITensorOperatorStream.h new file mode 100644 index 0000000..5319785 --- /dev/null +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/ITensorOperatorStream.h @@ -0,0 +1,298 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include + +#include "extensions/tensorops/core/CameraModel.h" +#include "extensions/tensorops/core/ComputeEngine.h" +#include "extensions/tensorops/core/Errors.h" +#include "extensions/tensorops/core/IImageWarp.h" +#include "extensions/tensorops/core/Image.h" +#include "extensions/tensorops/core/ImageUtils.h" + +namespace cvcore { +namespace tensor_ops { + +class NotImplementedException : public std::logic_error { + public: + NotImplementedException() + : std::logic_error("Method not yet implemented.") { + } +}; + +class ITensorOperatorStream { + public: + // Public Constructor(s)/Destructor + virtual ~ITensorOperatorStream() noexcept = default; + + // Public Accessor Method(s) + virtual std::error_code Status() noexcept = 0; + + virtual std::error_code GenerateWarpFromCameraModel(ImageWarp& warp, + const ImageGrid& grid, + const CameraModel& source, + const CameraIntrinsics& target) = 0; + + virtual std::error_code DestroyWarp(ImageWarp & warp) noexcept = 0; + + // Public Mutator Method(s) + virtual std::error_code Remap(Image & outputImage, const Image & inputImage, + const ImageWarp warp, + InterpolationType interpolation = INTERP_LINEAR, + BorderType border = BORDER_ZERO) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + virtual std::error_code Remap(Image & outputImage, const Image & inputImage, + const ImageWarp warp, + InterpolationType interpolation = INTERP_LINEAR, + BorderType border = BORDER_ZERO) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + virtual std::error_code Remap(Image & outputImage, + const Image & inputImage, + const ImageWarp warp, + InterpolationType interpolation = INTERP_LINEAR, + BorderType border = BORDER_ZERO) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + virtual std::error_code Remap(Image & outputImage, + const Image & inputImage, + const ImageWarp warp, + InterpolationType interpolation = INTERP_LINEAR, + BorderType border = BORDER_ZERO) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + virtual std::error_code Resize(Image & outputImage, + const Image & inputImage, + InterpolationType interpolation = INTERP_LINEAR, + BorderType border = BORDER_ZERO) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + virtual std::error_code Resize(Image & outputImage, + const Image & inputImage, + InterpolationType interpolation = INTERP_LINEAR, + BorderType border = BORDER_ZERO) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + virtual std::error_code Resize(Image & outputImage, + const Image & inputImage, + InterpolationType interpolation = INTERP_LINEAR, + BorderType border = BORDER_ZERO) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + virtual std::error_code Resize(Image & outputImage, + const Image & inputImage, + InterpolationType interpolation = INTERP_LINEAR, + BorderType border = BORDER_ZERO) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + virtual std::error_code Resize(Image & outputImage, + const Image & inputImage, + InterpolationType interpolation = INTERP_LINEAR, + BorderType border = BORDER_ZERO) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + virtual std::error_code Resize(Image & outputImage, + const Image & inputImage, + InterpolationType interpolation = INTERP_LINEAR, + BorderType border = BORDER_ZERO) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + virtual std::error_code Normalize(Image & outputImage, + const Image & inputImage) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + virtual std::error_code Normalize(Image & outputImage, + const Image & inputImage) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + virtual std::error_code Normalize(Image & outputImage, + const Image & inputImage) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + virtual std::error_code Normalize(Image & outputImage, + const Image & inputImage) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + virtual std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + virtual std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + virtual std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + virtual std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + virtual std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + virtual std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + virtual std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + virtual std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + virtual std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + virtual std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + virtual std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + virtual std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + virtual std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + virtual std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + virtual std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + virtual std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + virtual std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + virtual std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + virtual std::error_code StereoDisparityEstimator(Image & outputImage, + const Image & inputLeftImage, const Image & inputRightImage, + size_t windowSize, size_t maxDisparity) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + virtual std::error_code StereoDisparityEstimator(Image & outputImage, + const Image & inputLeftImage, const Image & inputRightImage, + size_t windowSize, size_t maxDisparity) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + virtual std::error_code StereoDisparityEstimator(Image & outputImage, + const Image & inputLeftImage, const Image & inputRightImage, + size_t windowSize, size_t maxDisparity) { + throw NotImplementedException(); + return make_error_code(ErrorCode::NOT_IMPLEMENTED); + } + + protected: + // Protected Constructor(s) + ITensorOperatorStream() = default; + ITensorOperatorStream(const ITensorOperatorStream&) = default; + ITensorOperatorStream(ITensorOperatorStream &&) noexcept = default; + + // Protected Operator(s) + ITensorOperatorStream& operator=(const ITensorOperatorStream&) = default; + ITensorOperatorStream& operator=(ITensorOperatorStream&&) noexcept = default; +}; + +using TensorOperatorStream = ITensorOperatorStream *; + +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/Image.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Image.h similarity index 63% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/Image.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Image.h index 263a699..57b0993 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/Image.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Image.h @@ -15,25 +15,24 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_IMAGE_H -#define CVCORE_IMAGE_H +#pragma once #include #include #include #include -#include "Memory.h" -#include "Tensor.h" +#include "extensions/tensorops/core/Memory.h" +#include "extensions/tensorops/core/Tensor.h" namespace cvcore { +namespace tensor_ops { /** * An enum. * Enum type for image type. */ -enum ImageType -{ +enum ImageType { Y_U8, /**< 8-bit unsigned gray. */ Y_U16, /**< 16-bit unsigned gray. */ Y_S8, /**< 8-bit signed gray. */ @@ -71,31 +70,33 @@ enum ImageType /** * Struct type for image preprocessing params */ -struct ImagePreProcessingParams -{ - ImageType imgType; /**< Input Image Type. */ - float pixelMean[3]; /**< Image Mean value offset for R,G,B channels. Default is 0.0f */ - float normalization[3]; /**< Scale or normalization values for R,G,B channels. Default is 1.0/255.0f */ - float stdDev[3]; /**< Standard deviation values for R,G,B channels. Default is 1.0f */ +struct ImagePreProcessingParams { + ImageType imgType; + // Image Mean value offset for R,G,B channels. Default is 0.0f + float pixelMean[3]; + // Scale or normalization values for R,G,B channels. Default is 1.0/255.0f + float normalization[3]; + // Standard deviation values for R,G,B channels. Default is 1.0f + float stdDev[3]; }; template -struct IsCompositeImage : std::integral_constant -{ +struct IsCompositeImage : std::integral_constant { }; template struct IsPlanarImage - : std::integral_constant -{ + : std::integral_constant { }; template -struct IsInterleavedImage : std::integral_constant::value && !IsPlanarImage::value> -{ +struct IsInterleavedImage : std::integral_constant::value && !IsPlanarImage::value> { }; /** @@ -105,272 +106,238 @@ template struct ImageTraits; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::HWC; static constexpr ChannelCount CC = ChannelCount::C1; static constexpr ChannelType CT = ChannelType::U8; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::NHWC; static constexpr ChannelCount CC = ChannelCount::C1; static constexpr ChannelType CT = ChannelType::U8; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::HWC; static constexpr ChannelCount CC = ChannelCount::C1; static constexpr ChannelType CT = ChannelType::U16; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::NHWC; static constexpr ChannelCount CC = ChannelCount::C1; static constexpr ChannelType CT = ChannelType::U16; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::HWC; static constexpr ChannelCount CC = ChannelCount::C1; static constexpr ChannelType CT = ChannelType::S8; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::NHWC; static constexpr ChannelCount CC = ChannelCount::C1; static constexpr ChannelType CT = ChannelType::S8; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::HWC; static constexpr ChannelCount CC = ChannelCount::C1; static constexpr ChannelType CT = ChannelType::S16; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::NHWC; static constexpr ChannelCount CC = ChannelCount::C1; static constexpr ChannelType CT = ChannelType::S16; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::HWC; static constexpr ChannelCount CC = ChannelCount::C1; static constexpr ChannelType CT = ChannelType::F32; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::NHWC; static constexpr ChannelCount CC = ChannelCount::C1; static constexpr ChannelType CT = ChannelType::F32; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::HWC; static constexpr ChannelCount CC = ChannelCount::C3; static constexpr ChannelType CT = ChannelType::U8; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::NHWC; static constexpr ChannelCount CC = ChannelCount::C3; static constexpr ChannelType CT = ChannelType::U8; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::HWC; static constexpr ChannelCount CC = ChannelCount::C3; static constexpr ChannelType CT = ChannelType::U16; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::NHWC; static constexpr ChannelCount CC = ChannelCount::C3; static constexpr ChannelType CT = ChannelType::U16; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::HWC; static constexpr ChannelCount CC = ChannelCount::C3; static constexpr ChannelType CT = ChannelType::F32; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::NHWC; static constexpr ChannelCount CC = ChannelCount::C3; static constexpr ChannelType CT = ChannelType::F32; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::HWC; static constexpr ChannelCount CC = ChannelCount::C3; static constexpr ChannelType CT = ChannelType::U8; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::NHWC; static constexpr ChannelCount CC = ChannelCount::C3; static constexpr ChannelType CT = ChannelType::U8; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::HWC; static constexpr ChannelCount CC = ChannelCount::C3; static constexpr ChannelType CT = ChannelType::U16; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::NHWC; static constexpr ChannelCount CC = ChannelCount::C3; static constexpr ChannelType CT = ChannelType::U16; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::HWC; static constexpr ChannelCount CC = ChannelCount::C3; static constexpr ChannelType CT = ChannelType::F32; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::NHWC; static constexpr ChannelCount CC = ChannelCount::C3; static constexpr ChannelType CT = ChannelType::F32; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::CHW; static constexpr ChannelCount CC = ChannelCount::C3; static constexpr ChannelType CT = ChannelType::U8; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::NCHW; static constexpr ChannelCount CC = ChannelCount::C3; static constexpr ChannelType CT = ChannelType::U8; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::CHW; static constexpr ChannelCount CC = ChannelCount::C3; static constexpr ChannelType CT = ChannelType::U16; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::NCHW; static constexpr ChannelCount CC = ChannelCount::C3; static constexpr ChannelType CT = ChannelType::U16; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::CHW; static constexpr ChannelCount CC = ChannelCount::C3; static constexpr ChannelType CT = ChannelType::F32; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::NCHW; static constexpr ChannelCount CC = ChannelCount::C3; static constexpr ChannelType CT = ChannelType::F32; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::CHW; static constexpr ChannelCount CC = ChannelCount::C3; static constexpr ChannelType CT = ChannelType::U8; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::NCHW; static constexpr ChannelCount CC = ChannelCount::C3; static constexpr ChannelType CT = ChannelType::U8; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::CHW; static constexpr ChannelCount CC = ChannelCount::C3; static constexpr ChannelType CT = ChannelType::U16; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::NCHW; static constexpr ChannelCount CC = ChannelCount::C3; static constexpr ChannelType CT = ChannelType::U16; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::CHW; static constexpr ChannelCount CC = ChannelCount::C3; static constexpr ChannelType CT = ChannelType::F32; }; template<> -struct ImageTraits -{ +struct ImageTraits { static constexpr TensorLayout TL = TensorLayout::NCHW; static constexpr ChannelCount CC = ChannelCount::C3; static constexpr ChannelType CT = ChannelType::F32; @@ -379,58 +346,48 @@ struct ImageTraits /** * Get the bytes of each element for a specific ImageType. */ -inline size_t GetImageElementSize(const ImageType type) -{ +inline size_t GetImageElementSize(const ImageType type) { size_t imageElementSize; - switch (type) - { - case ImageType::Y_U8: - case ImageType::Y_S8: - case ImageType::RGB_U8: - case ImageType::BGR_U8: - case ImageType::RGBA_U8: - case ImageType::PLANAR_RGB_U8: - case ImageType::PLANAR_BGR_U8: - case ImageType::PLANAR_RGBA_U8: - { - imageElementSize = 1; - break; - } - case ImageType::Y_U16: - case ImageType::Y_S16: - case ImageType::RGB_U16: - case ImageType::BGR_U16: - case ImageType::RGBA_U16: - case ImageType::PLANAR_RGB_U16: - case ImageType::PLANAR_BGR_U16: - case ImageType::PLANAR_RGBA_U16: - case ImageType::Y_F16: - case ImageType::RGB_F16: - case ImageType::BGR_F16: - case ImageType::RGBA_F16: - case ImageType::PLANAR_RGB_F16: - case ImageType::PLANAR_BGR_F16: - case ImageType::PLANAR_RGBA_F16: - { - imageElementSize = 2; - break; - } - case ImageType::Y_F32: - case ImageType::RGB_F32: - case ImageType::BGR_F32: - case ImageType::RGBA_F32: - case ImageType::PLANAR_RGB_F32: - case ImageType::PLANAR_BGR_F32: - case ImageType::PLANAR_RGBA_F32: - { - imageElementSize = 4; - break; - } - default: - { - imageElementSize = 0; - } + switch (type) { + case ImageType::Y_U8: + case ImageType::Y_S8: + case ImageType::RGB_U8: + case ImageType::BGR_U8: + case ImageType::RGBA_U8: + case ImageType::PLANAR_RGB_U8: + case ImageType::PLANAR_BGR_U8: + case ImageType::PLANAR_RGBA_U8: + imageElementSize = 1; + break; + case ImageType::Y_U16: + case ImageType::Y_S16: + case ImageType::RGB_U16: + case ImageType::BGR_U16: + case ImageType::RGBA_U16: + case ImageType::PLANAR_RGB_U16: + case ImageType::PLANAR_BGR_U16: + case ImageType::PLANAR_RGBA_U16: + case ImageType::Y_F16: + case ImageType::RGB_F16: + case ImageType::BGR_F16: + case ImageType::RGBA_F16: + case ImageType::PLANAR_RGB_F16: + case ImageType::PLANAR_BGR_F16: + case ImageType::PLANAR_RGBA_F16: + imageElementSize = 2; + break; + case ImageType::Y_F32: + case ImageType::RGB_F32: + case ImageType::BGR_F32: + case ImageType::RGBA_F32: + case ImageType::PLANAR_RGB_F32: + case ImageType::PLANAR_BGR_F32: + case ImageType::PLANAR_RGBA_F32: + imageElementSize = 4; + break; + default: + imageElementSize = 0; } return imageElementSize; @@ -439,341 +396,284 @@ inline size_t GetImageElementSize(const ImageType type) /** * Get the number of channels for a specific ImageType. */ -inline size_t GetImageChannelCount(const ImageType type) -{ +inline size_t GetImageChannelCount(const ImageType type) { size_t imageChannelCount; - switch (type) - { - case ImageType::Y_U8: - case ImageType::Y_U16: - case ImageType::Y_S8: - case ImageType::Y_S16: - case ImageType::Y_F16: - case ImageType::Y_F32: - { - imageChannelCount = 1; - break; - } - case ImageType::RGB_U8: - case ImageType::RGB_U16: - case ImageType::RGB_F16: - case ImageType::RGB_F32: - case ImageType::BGR_U8: - case ImageType::BGR_U16: - case ImageType::BGR_F16: - case ImageType::BGR_F32: - case ImageType::PLANAR_RGB_U8: - case ImageType::PLANAR_RGB_U16: - case ImageType::PLANAR_RGB_F16: - case ImageType::PLANAR_RGB_F32: - case ImageType::PLANAR_BGR_U8: - case ImageType::PLANAR_BGR_U16: - case ImageType::PLANAR_BGR_F16: - case ImageType::PLANAR_BGR_F32: - { - imageChannelCount = 3; - break; - } - case ImageType::RGBA_U8: - case ImageType::RGBA_U16: - case ImageType::RGBA_F16: - case ImageType::RGBA_F32: - case ImageType::PLANAR_RGBA_U8: - case ImageType::PLANAR_RGBA_U16: - case ImageType::PLANAR_RGBA_F16: - case ImageType::PLANAR_RGBA_F32: - { - imageChannelCount = 4; - break; - } - default: - { - imageChannelCount = 0; - } + switch (type) { + case ImageType::Y_U8: + case ImageType::Y_U16: + case ImageType::Y_S8: + case ImageType::Y_S16: + case ImageType::Y_F16: + case ImageType::Y_F32: + imageChannelCount = 1; + break; + case ImageType::RGB_U8: + case ImageType::RGB_U16: + case ImageType::RGB_F16: + case ImageType::RGB_F32: + case ImageType::BGR_U8: + case ImageType::BGR_U16: + case ImageType::BGR_F16: + case ImageType::BGR_F32: + case ImageType::PLANAR_RGB_U8: + case ImageType::PLANAR_RGB_U16: + case ImageType::PLANAR_RGB_F16: + case ImageType::PLANAR_RGB_F32: + case ImageType::PLANAR_BGR_U8: + case ImageType::PLANAR_BGR_U16: + case ImageType::PLANAR_BGR_F16: + case ImageType::PLANAR_BGR_F32: + imageChannelCount = 3; + break; + case ImageType::RGBA_U8: + case ImageType::RGBA_U16: + case ImageType::RGBA_F16: + case ImageType::RGBA_F32: + case ImageType::PLANAR_RGBA_U8: + case ImageType::PLANAR_RGBA_U16: + case ImageType::PLANAR_RGBA_F16: + case ImageType::PLANAR_RGBA_F32: + imageChannelCount = 4; + break; + default: + imageChannelCount = 0; } return imageChannelCount; -}; +} template -class Image -{ -}; +class Image {}; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image : public Tensor -{ +class Image : public Tensor { using Tensor::Tensor; }; template<> -class Image -{ -public: +class Image { + public: Image(std::size_t width, std::size_t height, bool isCPU = true) - : m_data(std::make_tuple(Y(width, height, isCPU), UV(width / 2, height / 2, isCPU))) - { + : m_data(std::make_tuple(Y(width, height, isCPU), UV(width / 2, height / 2, isCPU))) { assert(width % 2 == 0 && height % 2 == 0); } - Image(std::size_t width, std::size_t height, std::uint8_t *dataPtrLuma, std::uint8_t *dataPtrChroma, - bool isCPU = true) - : m_data(std::make_tuple(Y(width, height, dataPtrLuma, isCPU), UV(width / 2, height / 2, dataPtrChroma, isCPU))) - { + Image(std::size_t width, std::size_t height, std::uint8_t * dataPtrLuma, + std::uint8_t * dataPtrChroma, bool isCPU = true) + : m_data(std::make_tuple(Y(width, height, dataPtrLuma, isCPU), + UV(width / 2, height / 2, dataPtrChroma, isCPU))) { assert(width % 2 == 0 && height % 2 == 0); } - Image(std::size_t width, std::size_t height, std::size_t rowPitchLuma, std::size_t rowPitchChroma, - std::uint8_t *dataPtrLuma, std::uint8_t *dataPtrChroma, bool isCPU = true) + Image(std::size_t width, std::size_t height, std::size_t rowPitchLuma, + std::size_t rowPitchChroma, std::uint8_t* dataPtrLuma, + std::uint8_t* dataPtrChroma, bool isCPU = true) : m_data(std::make_tuple(Y(width, height, rowPitchLuma, dataPtrLuma, isCPU), - UV(width / 2, height / 2, rowPitchChroma, dataPtrChroma, isCPU))) - { + UV(width / 2, height / 2, rowPitchChroma, dataPtrChroma, isCPU))) { assert(width % 2 == 0 && height % 2 == 0); } - std::size_t getLumaWidth() const - { + std::size_t getLumaWidth() const { return std::get<0>(m_data).getWidth(); } - std::size_t getLumaHeight() const - { + std::size_t getLumaHeight() const { return std::get<0>(m_data).getHeight(); } - std::size_t getChromaWidth() const - { + std::size_t getChromaWidth() const { return std::get<1>(m_data).getWidth(); } - std::size_t getChromaHeight() const - { + std::size_t getChromaHeight() const { return std::get<1>(m_data).getHeight(); } - std::size_t getLumaStride(TensorDimension dim) const - { + std::size_t getLumaStride(TensorDimension dim) const { return std::get<0>(m_data).getStride(dim); } - std::size_t getChromaStride(TensorDimension dim) const - { + std::size_t getChromaStride(TensorDimension dim) const { return std::get<1>(m_data).getStride(dim); } - std::uint8_t *getLumaData() - { + std::uint8_t* getLumaData() { return std::get<0>(m_data).getData(); } - std::uint8_t *getChromaData() - { + std::uint8_t* getChromaData() { return std::get<1>(m_data).getData(); } - const std::uint8_t *getLumaData() const - { + const std::uint8_t* getLumaData() const { return std::get<0>(m_data).getData(); } - std::size_t getLumaDataSize() const - { + std::size_t getLumaDataSize() const { return std::get<0>(m_data).getDataSize(); } - const std::uint8_t *getChromaData() const - { + const std::uint8_t* getChromaData() const { return std::get<1>(m_data).getData(); } - std::size_t getChromaDataSize() const - { + std::size_t getChromaDataSize() const { return std::get<1>(m_data).getDataSize(); } - bool isCPU() const - { + bool isCPU() const { return std::get<0>(m_data).isCPU(); } - friend void Copy(Image &dst, const Image &src, cudaStream_t stream); + friend void Copy(Image & dst, const Image & src, cudaStream_t stream); -private: + private: using Y = Tensor; using UV = Tensor; @@ -781,113 +681,92 @@ class Image }; template<> -class Image -{ -public: +class Image { + public: Image(std::size_t width, std::size_t height, bool isCPU = true) - : m_data(std::make_tuple(Y(width, height, isCPU), UV(width, height, isCPU))) - { - } + : m_data(std::make_tuple(Y(width, height, isCPU), UV(width, height, isCPU))) {} - Image(std::size_t width, std::size_t height, std::uint8_t *dataPtrLuma, std::uint8_t *dataPtrChroma, - bool isCPU = true) - : m_data(std::make_tuple(Y(width, height, dataPtrLuma, isCPU), UV(width, height, dataPtrChroma, isCPU))) - { - } + Image(std::size_t width, std::size_t height, std::uint8_t *dataPtrLuma, + std::uint8_t *dataPtrChroma, bool isCPU = true) + : m_data(std::make_tuple(Y(width, height, dataPtrLuma, isCPU), + UV(width, height, dataPtrChroma, isCPU))) {} - Image(std::size_t width, std::size_t height, std::size_t rowPitchLuma, std::size_t rowPitchChroma, - std::uint8_t *dataPtrLuma, std::uint8_t *dataPtrChroma, bool isCPU = true) + Image(std::size_t width, std::size_t height, std::size_t rowPitchLuma, + std::size_t rowPitchChroma, std::uint8_t* dataPtrLuma, + std::uint8_t* dataPtrChroma, bool isCPU = true) : m_data(std::make_tuple(Y(width, height, rowPitchLuma, dataPtrLuma, isCPU), - UV(width, height, rowPitchChroma, dataPtrChroma, isCPU))) - { - } + UV(width, height, rowPitchChroma, dataPtrChroma, isCPU))) {} - std::size_t getLumaWidth() const - { + std::size_t getLumaWidth() const { return std::get<0>(m_data).getWidth(); } - std::size_t getLumaHeight() const - { + std::size_t getLumaHeight() const { return std::get<0>(m_data).getHeight(); } - std::size_t getChromaWidth() const - { + std::size_t getChromaWidth() const { return std::get<1>(m_data).getWidth(); } - std::size_t getChromaHeight() const - { + std::size_t getChromaHeight() const { return std::get<1>(m_data).getHeight(); } - std::size_t getLumaStride(TensorDimension dim) const - { + std::size_t getLumaStride(TensorDimension dim) const { return std::get<0>(m_data).getStride(dim); } - std::size_t getChromaStride(TensorDimension dim) const - { + std::size_t getChromaStride(TensorDimension dim) const { return std::get<1>(m_data).getStride(dim); } - std::uint8_t *getLumaData() - { + std::uint8_t* getLumaData() { return std::get<0>(m_data).getData(); } - const std::uint8_t *getLumaData() const - { + const std::uint8_t* getLumaData() const { return std::get<0>(m_data).getData(); } - std::size_t getLumaDataSize() const - { + std::size_t getLumaDataSize() const { return std::get<0>(m_data).getDataSize(); } - std::uint8_t *getChromaData() - { + std::uint8_t* getChromaData() { return std::get<1>(m_data).getData(); } - const std::uint8_t *getChromaData() const - { + const std::uint8_t* getChromaData() const { return std::get<1>(m_data).getData(); } - std::size_t getChromaDataSize() const - { + std::size_t getChromaDataSize() const { return std::get<1>(m_data).getDataSize(); } - bool isCPU() const - { + bool isCPU() const { return std::get<0>(m_data).isCPU(); } - friend void Copy(Image &dst, const Image &src, cudaStream_t stream); + friend void Copy(Image & dst, const Image & src, cudaStream_t stream); -private: + private: using Y = Tensor; using UV = Tensor; std::tuple m_data; }; -void inline Copy(Image &dst, const Image &src, cudaStream_t stream = 0) -{ +void inline Copy(Image & dst, const Image & src, cudaStream_t stream = 0) { Copy(std::get<0>(dst.m_data), std::get<0>(src.m_data), stream); Copy(std::get<1>(dst.m_data), std::get<1>(src.m_data), stream); } -void inline Copy(Image &dst, const Image &src, cudaStream_t stream = 0) -{ +void inline Copy(Image & dst, const Image & src, cudaStream_t stream = 0) { Copy(std::get<0>(dst.m_data), std::get<0>(src.m_data), stream); Copy(std::get<1>(dst.m_data), std::get<1>(src.m_data), stream); } -} // namespace cvcore - -#endif // CVCORE_IMAGE_H +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/ImageUtils.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/ImageUtils.h similarity index 64% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/ImageUtils.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/ImageUtils.h index e46cd42..0267f06 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/ImageUtils.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/ImageUtils.h @@ -15,24 +15,22 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_IMAGE_UTILS_H -#define CVCORE_IMAGE_UTILS_H - -#include +#pragma once #include +#include -#include "cv/core/BBox.h" -#include "cv/core/Tensor.h" +#include "extensions/tensorops/core/BBox.h" +#include "extensions/tensorops/core/Tensor.h" -namespace cvcore { namespace tensor_ops { +namespace cvcore { +namespace tensor_ops { /** * An enum. * Enum type for color conversion type. */ -enum ColorConversionType -{ +enum ColorConversionType { BGR2RGB, /**< convert BGR to RGB. */ RGB2BGR, /**< convert RGB to BGR. */ BGR2GRAY, /**< convert BGR to Grayscale. */ @@ -45,8 +43,7 @@ enum ColorConversionType * An enum. * Enum type for resize interpolation type. */ -enum InterpolationType -{ +enum InterpolationType { INTERP_NEAREST, /**< nearest interpolation. */ INTERP_LINEAR, /**< linear interpolation. */ INTERP_CUBIC_BSPLINE, /**< cubic bspline interpolation. */ @@ -57,8 +54,7 @@ enum InterpolationType * An enum. * Enum type for resize interpolation type. */ -enum BorderType -{ +enum BorderType { BORDER_ZERO, BORDER_REPEAT, BORDER_REVERSE, @@ -75,8 +71,9 @@ enum BorderType * @param keep_aspect_ratio whether to keep aspect ratio. * @param stream specified cuda stream. */ -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio = false, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio = false, InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** * Image batch resizing for one channel NHWC uint_8 type Tensor. @@ -86,8 +83,9 @@ void Resize(Tensor &dst, const Tensor &src, bool keep_ * @param keep_aspect_ratio whether to keep aspect ratio. * @param stream specified cuda stream. */ -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio = false, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio = false, InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** * Image resizing of a region of interest for one channel HWC format uint_8 type Tensor. @@ -98,8 +96,9 @@ void Resize(Tensor &dst, const Tensor &src, bool kee * @param type interpolation type. * @param stream specified cuda stream. */ -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &dstROI, const BBox &srcROI, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& dstROI, const BBox& srcROI, InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** * Image resizing of a region of interest for one channel HWC format uint_8 type Tensor. @@ -109,8 +108,8 @@ void CropAndResize(Tensor &dst, const Tensor &src, con * @param type interpolation type. * @param stream specified cuda stream. */ -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &srcROI, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& srcROI, InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); /** * Image resizing for one channel HWC format uint_16 type Tensor. @@ -120,8 +119,9 @@ void CropAndResize(Tensor &dst, const Tensor &src, con * @param keep_aspect_ratio whether to keep aspect ratio. * @param stream specified cuda stream. */ -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio = false, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio = false, InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** * Image batch resizing for one channel HWC format uint_16 type Tensor. @@ -131,8 +131,9 @@ void Resize(Tensor &dst, const Tensor &src, bool kee * @param keep_aspect_ratio whether to keep aspect ratio. * @param stream specified cuda stream. */ -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio = false, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio = false, InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** * Image resizing of a region of interest for one channel HWC format uint_16 type Tensor. @@ -143,8 +144,9 @@ void Resize(Tensor &dst, const Tensor &src, bool k * @param type interpolation type. * @param stream specified cuda stream. */ -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &dstROI, const BBox &srcROI, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& dstROI, const BBox& srcROI, InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** * Image resizing of a region of interest for one channel HWC format uint_16 type Tensor. @@ -154,8 +156,8 @@ void CropAndResize(Tensor &dst, const Tensor &src, c * @param type interpolation type. * @param stream specified cuda stream. */ -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &srcROI, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& srcROI, InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); /** * Image resizing for one channel HWC format FP32 type Tensor. @@ -165,8 +167,9 @@ void CropAndResize(Tensor &dst, const Tensor &src, c * @param keep_aspect_ratio whether to keep aspect ratio. * @param stream specified cuda stream. */ -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio = false, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio = false, InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** * Image batch resizing for one channel HWC format FP32 type Tensor. @@ -176,8 +179,9 @@ void Resize(Tensor &dst, const Tensor &src, bool kee * @param keep_aspect_ratio whether to keep aspect ratio. * @param stream specified cuda stream. */ -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio = false, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio = false, InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** * Image resizing of a region of interest for one channel HWC format FP32 type Tensor. @@ -188,8 +192,9 @@ void Resize(Tensor &dst, const Tensor &src, bool k * @param type interpolation type. * @param stream specified cuda stream. */ -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &dstROI, const BBox &srcROI, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& dstROI, const BBox& srcROI, InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** * Image resizing of a region of interest for one channel HWC format FP32 type Tensor. @@ -199,8 +204,8 @@ void CropAndResize(Tensor &dst, const Tensor &src, c * @param type interpolation type. * @param stream specified cuda stream. */ -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &srcROI, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& srcROI, InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); /** * Image resizing for one channel CHW format uint_8 type Tensor. @@ -210,8 +215,9 @@ void CropAndResize(Tensor &dst, const Tensor &src, c * @param keep_aspect_ratio whether to keep aspect ratio. * @param stream specified cuda stream. */ -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio = false, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio = false, InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** * Image resizing of a region of interest for one channel CHW format uint_8 type Tensor. @@ -222,8 +228,9 @@ void Resize(Tensor &dst, const Tensor &src, bool keep_ * @param type interpolation type. * @param stream specified cuda stream. */ -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &dstROI, const BBox &srcROI, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& dstROI, const BBox& srcROI, InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** * Image resizing of a region of interest for one channel CHW format uint_8 type Tensor. @@ -233,8 +240,8 @@ void CropAndResize(Tensor &dst, const Tensor &src, con * @param type interpolation type. * @param stream specified cuda stream. */ -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &srcROI, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& srcROI, InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); /** * Image resizing for one channel CHW format uint_16 type Tensor. @@ -244,8 +251,9 @@ void CropAndResize(Tensor &dst, const Tensor &src, con * @param keep_aspect_ratio whether to keep aspect ratio. * @param stream specified cuda stream. */ -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio = false, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio = false, InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** * Image resizing of a region of interest for one channel CHW format uint_16 type Tensor. @@ -256,8 +264,9 @@ void Resize(Tensor &dst, const Tensor &src, bool kee * @param type interpolation type. * @param stream specified cuda stream. */ -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &dstROI, const BBox &srcROI, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& dstROI, const BBox& srcROI, InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** * Image resizing of a region of interest for one channel CHW format uint_16 type Tensor. @@ -267,8 +276,8 @@ void CropAndResize(Tensor &dst, const Tensor &src, c * @param type interpolation type. * @param stream specified cuda stream. */ -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &srcROI, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& srcROI, InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); /** * Image resizing for one channel CHW format FP32 type Tensor. @@ -278,8 +287,9 @@ void CropAndResize(Tensor &dst, const Tensor &src, c * @param keep_aspect_ratio whether to keep aspect ratio. * @param stream specified cuda stream. */ -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio = false, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio = false, InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** * Image resizing of a region of interest for one channel CHW format FP32 type Tensor. @@ -290,8 +300,9 @@ void Resize(Tensor &dst, const Tensor &src, bool kee * @param type interpolation type. * @param stream specified cuda stream. */ -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &dstROI, const BBox &srcROI, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& dstROI, const BBox& srcROI, InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** * Image resizing of a region of interest for one channel CHW format FP32 type Tensor. @@ -301,8 +312,8 @@ void CropAndResize(Tensor &dst, const Tensor &src, c * @param type interpolation type. * @param stream specified cuda stream. */ -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &srcROI, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& srcROI, InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); /** * Image resizing for three channels interleaved uint_8 type Tensor. @@ -312,8 +323,9 @@ void CropAndResize(Tensor &dst, const Tensor &src, c * @param keep_aspect_ratio whether to keep aspect ratio. * @param stream specified cuda stream. */ -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio = false, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio = false, InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** * Image batch resizing for three channels interleaved uint_8 type Tensor. @@ -323,8 +335,9 @@ void Resize(Tensor &dst, const Tensor &src, bool keep_ * @param keep_aspect_ratio whether to keep aspect ratio. * @param stream specified cuda stream. */ -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio = false, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio = false, InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** * Image resizing of a region of intesrest for three channel HWC format uint_8 type Tensor. @@ -335,8 +348,9 @@ void Resize(Tensor &dst, const Tensor &src, bool kee * @param type interpolation type. * @param stream specified cuda stream. */ -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &dstROI, const BBox &srcROI, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& dstROI, const BBox& srcROI, + InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); /** * Image resizing of a region of intesrest for three channel HWC format uint_8 type Tensor. @@ -346,8 +360,8 @@ void CropAndResize(Tensor &dst, const Tensor &src, con * @param type interpolation type. * @param stream specified cuda stream. */ -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &srcROI, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& srcROI, InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); /** * Image resizing for three channels interleaved uint_16 type Tensor. @@ -357,8 +371,9 @@ void CropAndResize(Tensor &dst, const Tensor &src, con * @param keep_aspect_ratio whether to keep aspect ratio. * @param stream specified cuda stream. */ -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio = false, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio = false, InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** * Image batch resizing for three channels interleaved uint_16 type Tensor. @@ -368,8 +383,9 @@ void Resize(Tensor &dst, const Tensor &src, bool kee * @param keep_aspect_ratio whether to keep aspect ratio. * @param stream specified cuda stream. */ -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio = false, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio = false, InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** * Image resizing of a region of intesrest for three channel HWC format uint_16 type Tensor. @@ -380,8 +396,9 @@ void Resize(Tensor &dst, const Tensor &src, bool k * @param type interpolation type. * @param stream specified cuda stream. */ -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &dstROI, const BBox &srcROI, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& dstROI, const BBox& srcROI, + InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); /** * Image resizing of a region of intesrest for three channel HWC format uint_16 type Tensor. @@ -391,8 +408,8 @@ void CropAndResize(Tensor &dst, const Tensor &src, c * @param type interpolation type. * @param stream specified cuda stream. */ -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &srcROI, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& srcROI, InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); /** * Image resizing for three channels interleaved float type Tensor. @@ -402,8 +419,9 @@ void CropAndResize(Tensor &dst, const Tensor &src, c * @param keep_aspect_ratio whether to keep aspect ratio. * @param stream specified cuda stream. */ -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio = false, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio = false, + InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); /** * Image batch resizing for three channels interleaved float type Tensor. @@ -413,8 +431,9 @@ void Resize(Tensor &dst, const Tensor &src, bool kee * @param keep_aspect_ratio whether to keep aspect ratio. * @param stream specified cuda stream. */ -void Resize(Tensor &dst, const Tensor &src, bool keep_aspect_ratio = false, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool Resize(Tensor& dst, const Tensor& src, + bool keep_aspect_ratio = false, + InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); /** * Image resizing of a region of intesrest for three channel HWC format float type Tensor. @@ -425,8 +444,9 @@ void Resize(Tensor &dst, const Tensor &src, bool k * @param type interpolation type. * @param stream specified cuda stream. */ -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &dstROI, const BBox &srcROI, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& dstROI, const BBox& srcROI, + InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); /** * Image resizing of a region of intesrest for three channel HWC format float type Tensor. @@ -436,8 +456,9 @@ void CropAndResize(Tensor &dst, const Tensor &src, c * @param type interpolation type. * @param stream specified cuda stream. */ -void CropAndResize(Tensor &dst, const Tensor &src, const BBox &srcROI, - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool CropAndResize(Tensor& dst, const Tensor& src, + const BBox& srcROI, InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** * Crop a region of interest for one channel HWC format uint_8 type Tensor. @@ -446,7 +467,8 @@ void CropAndResize(Tensor &dst, const Tensor &src, c * @param srcROI source crop region. * @param stream specified cuda stream. */ -void Crop(Tensor &dst, const Tensor &src, const BBox &srcROI, cudaStream_t stream = 0); +bool Crop(Tensor& dst, const Tensor& src, + const BBox& srcROI, cudaStream_t stream = 0); /** * Crop a region of interest for one channel HWC format uint_16 type Tensor. @@ -455,7 +477,8 @@ void Crop(Tensor &dst, const Tensor &src, const BBox & * @param srcROI source crop region. * @param stream specified cuda stream. */ -void Crop(Tensor &dst, const Tensor &src, const BBox &srcROI, cudaStream_t stream = 0); +bool Crop(Tensor& dst, const Tensor& src, + const BBox& srcROI, cudaStream_t stream = 0); /** * Crop a region of interest for one channel HWC format float type Tensor. @@ -464,7 +487,8 @@ void Crop(Tensor &dst, const Tensor &src, const BBox * @param srcROI source crop region. * @param stream specified cuda stream. */ -void Crop(Tensor &dst, const Tensor &src, const BBox &srcROI, cudaStream_t stream = 0); +bool Crop(Tensor& dst, const Tensor& src, + const BBox& srcROI, cudaStream_t stream = 0); /** * Crop a region of interest for one channel CHW format uint_8 type Tensor. @@ -473,7 +497,8 @@ void Crop(Tensor &dst, const Tensor &src, const BBox * @param srcROI source crop region. * @param stream specified cuda stream. */ -void Crop(Tensor &dst, const Tensor &src, const BBox &srcROI, cudaStream_t stream = 0); +bool Crop(Tensor& dst, const Tensor& src, + const BBox& srcROI, cudaStream_t stream = 0); /** * Crop a region of interest for one channel CHW format uint_16 type Tensor. @@ -482,7 +507,8 @@ void Crop(Tensor &dst, const Tensor &src, const BBox & * @param srcROI source crop region. * @param stream specified cuda stream. */ -void Crop(Tensor &dst, const Tensor &src, const BBox &srcROI, cudaStream_t stream = 0); +bool Crop(Tensor& dst, const Tensor& src, + const BBox& srcROI, cudaStream_t stream = 0); /** * Crop a region of interest for one channel CHW format float type Tensor. @@ -491,7 +517,8 @@ void Crop(Tensor &dst, const Tensor &src, const BBox * @param srcROI source crop region. * @param stream specified cuda stream. */ -void Crop(Tensor &dst, const Tensor &src, const BBox &srcROI, cudaStream_t stream = 0); +bool Crop(Tensor& dst, const Tensor& src, + const BBox& srcROI, cudaStream_t stream = 0); /** * Crop a region of interest for three channels HWC format uint_8 type Tensor. @@ -500,7 +527,8 @@ void Crop(Tensor &dst, const Tensor &src, const BBox * @param srcROI source crop region. * @param stream specified cuda stream. */ -void Crop(Tensor &dst, const Tensor &src, const BBox &srcROI, cudaStream_t stream = 0); +bool Crop(Tensor& dst, const Tensor& src, + const BBox& srcROI, cudaStream_t stream = 0); /** * Crop a region of interest for three channels HWC format uint_16 type Tensor. @@ -509,7 +537,8 @@ void Crop(Tensor &dst, const Tensor &src, const BBox & * @param srcROI source crop region. * @param stream specified cuda stream. */ -void Crop(Tensor &dst, const Tensor &src, const BBox &srcROI, cudaStream_t stream = 0); +bool Crop(Tensor& dst, const Tensor& src, + const BBox& srcROI, cudaStream_t stream = 0); /** * Crop a region of interest for three channels HWC format float type Tensor. @@ -518,7 +547,8 @@ void Crop(Tensor &dst, const Tensor &src, const BBox * @param srcROI source crop region. * @param stream specified cuda stream. */ -void Crop(Tensor &dst, const Tensor &src, const BBox &srcROI, cudaStream_t stream = 0); +bool Crop(Tensor& dst, const Tensor& src, + const BBox& srcROI, cudaStream_t stream = 0); /** * Apply a perspective transformation to one channel HWC format uint_8 type Tensor. @@ -528,8 +558,9 @@ void Crop(Tensor &dst, const Tensor &src, const BBox * @param type interpolation type. * @param stream specified cuda stream. */ -void WarpPerspective(Tensor &dst, const Tensor &src, const double coeffs[3][3], - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool WarpPerspective(Tensor& dst, const Tensor& src, + const double coeffs[3][3], InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** * Apply a perspective transformation to one channel HWC format uint_16 type Tensor. @@ -539,8 +570,9 @@ void WarpPerspective(Tensor &dst, const Tensor &src, c * @param type interpolation type. * @param stream specified cuda stream. */ -void WarpPerspective(Tensor &dst, const Tensor &src, const double coeffs[3][3], - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool WarpPerspective(Tensor& dst, const Tensor& src, + const double coeffs[3][3], InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** * Apply a perspective transformation to one channel HWC format float type Tensor. @@ -550,8 +582,9 @@ void WarpPerspective(Tensor &dst, const Tensor &src, * @param type interpolation type. * @param stream specified cuda stream. */ -void WarpPerspective(Tensor &dst, const Tensor &src, const double coeffs[3][3], - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool WarpPerspective(Tensor& dst, const Tensor& src, + const double coeffs[3][3], InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** * Apply a perspective transformation to three channels HWC format uint_8 type Tensor. @@ -561,8 +594,9 @@ void WarpPerspective(Tensor &dst, const Tensor &src, * @param type interpolation type. * @param stream specified cuda stream. */ -void WarpPerspective(Tensor &dst, const Tensor &src, const double coeffs[3][3], - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool WarpPerspective(Tensor& dst, const Tensor& src, + const double coeffs[3][3], InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** * Apply a perspective transformation to three channels HWC format uint_16 type Tensor. @@ -572,8 +606,9 @@ void WarpPerspective(Tensor &dst, const Tensor &src, c * @param type interpolation type. * @param stream specified cuda stream. */ -void WarpPerspective(Tensor &dst, const Tensor &src, const double coeffs[3][3], - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool WarpPerspective(Tensor& dst, const Tensor& src, + const double coeffs[3][3], InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** * Apply a perspective transformation to three channels HWC format float type Tensor. @@ -583,8 +618,9 @@ void WarpPerspective(Tensor &dst, const Tensor &src, * @param type interpolation type. * @param stream specified cuda stream. */ -void WarpPerspective(Tensor &dst, const Tensor &src, const double coeffs[3][3], - InterpolationType type = INTERP_LINEAR, cudaStream_t stream = 0); +bool WarpPerspective(Tensor& dst, const Tensor& src, + const double coeffs[3][3], InterpolationType type = INTERP_LINEAR, + cudaStream_t stream = 0); /** Color conversion between two three channels interleaved uint_8 type Tensor. * @param dst destination tensor. @@ -592,8 +628,8 @@ void WarpPerspective(Tensor &dst, const Tensor &src, * @param type color conversion type. * @param stream specified cuda stream. */ -void ConvertColorFormat(Tensor &dst, const Tensor &src, ColorConversionType type, - cudaStream_t stream = 0); +bool ConvertColorFormat(Tensor& dst, const Tensor& src, + ColorConversionType type, cudaStream_t stream = 0); /** Batch color conversion between three channels interleaved uint_8 type Tensors. * @param dst destination tensor. @@ -601,8 +637,8 @@ void ConvertColorFormat(Tensor &dst, const Tensor &src * @param type color conversion type. * @param stream specified cuda stream. */ -void ConvertColorFormat(Tensor &dst, const Tensor &src, ColorConversionType type, - cudaStream_t stream = 0); +bool ConvertColorFormat(Tensor& dst, const Tensor& src, + ColorConversionType type, cudaStream_t stream = 0); /** Color conversion between two three channels interleaved uint_16 type Tensor. * @param dst destination tensor. @@ -610,8 +646,8 @@ void ConvertColorFormat(Tensor &dst, const Tensor &s * @param type color conversion type. * @param stream specified cuda stream. */ -void ConvertColorFormat(Tensor &dst, const Tensor &src, ColorConversionType type, - cudaStream_t stream = 0); +bool ConvertColorFormat(Tensor& dst, const Tensor& src, + ColorConversionType type, cudaStream_t stream = 0); /** Batch color conversion between three channels interleaved uint_16 type Tensors. * @param dst destination tensor. @@ -619,8 +655,8 @@ void ConvertColorFormat(Tensor &dst, const Tensor &s * @param type color conversion type. * @param stream specified cuda stream. */ -void ConvertColorFormat(Tensor &dst, const Tensor &src, ColorConversionType type, - cudaStream_t stream = 0); +bool ConvertColorFormat(Tensor& dst, const Tensor& src, + ColorConversionType type, cudaStream_t stream = 0); /** Color conversion between two three channels interleaved float type Tensor. * @param dst destination tensor. @@ -628,8 +664,8 @@ void ConvertColorFormat(Tensor &dst, const Tensor * @param type color conversion type. * @param stream specified cuda stream. */ -void ConvertColorFormat(Tensor &dst, const Tensor &src, ColorConversionType type, - cudaStream_t stream = 0); +bool ConvertColorFormat(Tensor& dst, const Tensor& src, + ColorConversionType type, cudaStream_t stream = 0); /** Batch color conversion between three channels interleaved float type Tensors. * @param dst destination tensor. @@ -637,8 +673,8 @@ void ConvertColorFormat(Tensor &dst, const Tensor &s * @param type color conversion type. * @param stream specified cuda stream. */ -void ConvertColorFormat(Tensor &dst, const Tensor &src, ColorConversionType type, - cudaStream_t stream = 0); +bool ConvertColorFormat(Tensor& dst, const Tensor& src, + ColorConversionType type, cudaStream_t stream = 0); /** Color conversion from three channels interleaved uint_8 type Tensor to one channel Tensor. * @param dst destination tensor. @@ -646,8 +682,8 @@ void ConvertColorFormat(Tensor &dst, const Tensor * @param type color conversion type. * @param stream specified cuda stream. */ -void ConvertColorFormat(Tensor &dst, const Tensor &src, ColorConversionType type, - cudaStream_t stream = 0); +bool ConvertColorFormat(Tensor& dst, const Tensor& src, + ColorConversionType type, cudaStream_t stream = 0); /** Color conversion from three channels interleaved uint_16 type Tensor to one channel Tensor. * @param dst destination tensor. @@ -655,8 +691,8 @@ void ConvertColorFormat(Tensor &dst, const Tensor &src * @param type color conversion type. * @param stream specified cuda stream. */ -void ConvertColorFormat(Tensor &dst, const Tensor &src, ColorConversionType type, - cudaStream_t stream = 0); +bool ConvertColorFormat(Tensor& dst, const Tensor& src, + ColorConversionType type, cudaStream_t stream = 0); /** Color conversion from three channels interleaved float type Tensor to one channel Tensor. * @param dst destination tensor. @@ -664,8 +700,8 @@ void ConvertColorFormat(Tensor &dst, const Tensor &s * @param type color conversion type. * @param stream specified cuda stream. */ -void ConvertColorFormat(Tensor &dst, const Tensor &src, ColorConversionType type, - cudaStream_t stream = 0); +bool ConvertColorFormat(Tensor& dst, const Tensor& src, + ColorConversionType type, cudaStream_t stream = 0); /** Color conversion from one channel interleaved uint_8 type Tensor to three channels Tensor. * @param dst destination tensor. @@ -673,8 +709,8 @@ void ConvertColorFormat(Tensor &dst, const Tensor &s * @param type color conversion type. * @param stream specified cuda stream. */ -void ConvertColorFormat(Tensor &dst, const Tensor &src, ColorConversionType type, - cudaStream_t stream = 0); +bool ConvertColorFormat(Tensor& dst, const Tensor& src, + ColorConversionType type, cudaStream_t stream = 0); /** Color conversion from one channel interleaved uint_16 type Tensor to three channels Tensor. * @param dst destination tensor. @@ -682,8 +718,8 @@ void ConvertColorFormat(Tensor &dst, const Tensor &src * @param type color conversion type. * @param stream specified cuda stream. */ -void ConvertColorFormat(Tensor &dst, const Tensor &src, ColorConversionType type, - cudaStream_t stream = 0); +bool ConvertColorFormat(Tensor& dst, const Tensor& src, + ColorConversionType type, cudaStream_t stream = 0); /** Color conversion from one channel interleaved float type Tensor to three channels Tensor. * @param dst destination tensor. @@ -691,8 +727,8 @@ void ConvertColorFormat(Tensor &dst, const Tensor &s * @param type color conversion type. * @param stream specified cuda stream. */ -void ConvertColorFormat(Tensor &dst, const Tensor &src, ColorConversionType type, - cudaStream_t stream = 0); +bool ConvertColorFormat(Tensor& dst, const Tensor& src, + ColorConversionType type, cudaStream_t stream = 0); /** Convert bit depth from F32 to U8 of a single channel channel Tensor. * @param dst destination tensor. @@ -700,7 +736,8 @@ void ConvertColorFormat(Tensor &dst, const Tensor &s * @param scale multiply the pixel values by a factor. * @param stream specified cuda stream. */ -void ConvertBitDepth(Tensor &dst, Tensor &src, const float scale, cudaStream_t stream = 0); +bool ConvertBitDepth(Tensor& dst, Tensor& src, const float scale, + cudaStream_t stream = 0); /** Convert bit depth from F32 to U8 of a N * single channel Tensor. * @param dst destination tensor. @@ -708,7 +745,8 @@ void ConvertBitDepth(Tensor &dst, Tensor &src, const * @param scale multiply the pixel values by a factor. * @param stream specified cuda stream. */ -void ConvertBitDepth(Tensor &dst, Tensor &src, const float scale, cudaStream_t stream = 0); +bool ConvertBitDepth(Tensor& dst, Tensor& src, const float scale, + cudaStream_t stream = 0); /** * Normalization for three channels interleaved uint8_t type Tensor. @@ -719,8 +757,8 @@ void ConvertBitDepth(Tensor &dst, Tensor &src, cons * @param offset offset constant for normalization. * @stream specified cuda stream. */ -void Normalize(Tensor &dst, const Tensor &src, const float scale[3], const float offset[3], - cudaStream_t stream = 0); +bool Normalize(Tensor& dst, const Tensor& src, const float scale[3], + const float offset[3], cudaStream_t stream = 0); /** * Batch normalization for three channels interleaved uint8_t type Tensor. @@ -731,8 +769,8 @@ void Normalize(Tensor &dst, const Tensor &src, const * @param offset offset constant for normalization. * @stream specified cuda stream. */ -void Normalize(Tensor &dst, const Tensor &src, const float scale[3], const float offset[3], - cudaStream_t stream = 0); +bool Normalize(Tensor& dst, const Tensor& src, const float scale[3], + const float offset[3], cudaStream_t stream = 0); /** * Normalization for three channels interleaved uint16_t type Tensor. @@ -743,8 +781,8 @@ void Normalize(Tensor &dst, const Tensor &src, cons * @param offset offset constant for normalization. * @stream specified cuda stream. */ -void Normalize(Tensor &dst, const Tensor &src, const float scale[3], const float offset[3], - cudaStream_t stream = 0); +bool Normalize(Tensor& dst, const Tensor& src, const float scale[3], + const float offset[3], cudaStream_t stream = 0); /** * Batch normalization for three channels interleaved uint16_t type Tensor. @@ -755,8 +793,8 @@ void Normalize(Tensor &dst, const Tensor &src, const * @param offset offset constant for normalization. * @stream specified cuda stream. */ -void Normalize(Tensor &dst, const Tensor &src, const float scale[3], - const float offset[3], cudaStream_t stream = 0); +bool Normalize(Tensor& dst, const Tensor& src, const float scale[3], + const float offset[3], cudaStream_t stream = 0); /** * Normalization for three channels interleaved float type Tensor. @@ -767,8 +805,8 @@ void Normalize(Tensor &dst, const Tensor &src, con * @param offset offset constant for normalization. * @stream specified cuda stream. */ -void Normalize(Tensor &dst, const Tensor &src, const float scale[3], const float offset[3], - cudaStream_t stream = 0); +bool Normalize(Tensor& dst, const Tensor& src, const float scale[3], + const float offset[3], cudaStream_t stream = 0); /** * Batch normalization for three channels interleaved float type Tensor. @@ -779,8 +817,8 @@ void Normalize(Tensor &dst, const Tensor &src, const * @param offset offset constant for normalization. * @stream specified cuda stream. */ -void Normalize(Tensor &dst, const Tensor &src, const float scale[3], - const float offset[3], cudaStream_t stream = 0); +bool Normalize(Tensor& dst, const Tensor& src, const float scale[3], + const float offset[3], cudaStream_t stream = 0); /** * Normalization for one channel interleaved uint8_t type Tensor. @@ -791,8 +829,8 @@ void Normalize(Tensor &dst, const Tensor &src, con * @param offset offset constant for normalization. * @stream specified cuda stream. */ -void Normalize(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream = 0); +bool Normalize(Tensor& dst, const Tensor& src, const float scale, + const float offset, cudaStream_t stream = 0); /** * Batch normalization for one channel interleaved uint8_t type Tensor. @@ -803,8 +841,8 @@ void Normalize(Tensor &dst, const Tensor &src, const * @param offset offset constant for normalization. * @stream specified cuda stream. */ -void Normalize(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream = 0); +bool Normalize(Tensor& dst, const Tensor& src, const float scale, + const float offset, cudaStream_t stream = 0); /** * Normalization for one channel interleaved uint16_t type Tensor. @@ -815,8 +853,8 @@ void Normalize(Tensor &dst, const Tensor &src, cons * @param offset offset constant for normalization. * @stream specified cuda stream. */ -void Normalize(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream = 0); +bool Normalize(Tensor& dst, const Tensor& src, const float scale, + const float offset, cudaStream_t stream = 0); /** * Batch normalization for one channel interleaved uint16_t type Tensor. @@ -827,8 +865,8 @@ void Normalize(Tensor &dst, const Tensor &src, const * @param offset offset constant for normalization. * @stream specified cuda stream. */ -void Normalize(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream = 0); +bool Normalize(Tensor& dst, const Tensor& src, const float scale, + const float offset, cudaStream_t stream = 0); /** * Normalization for one channel interleaved float type Tensor. @@ -839,8 +877,8 @@ void Normalize(Tensor &dst, const Tensor &src, con * @param offset offset constant for normalization. * @stream specified cuda stream. */ -void Normalize(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream = 0); +bool Normalize(Tensor& dst, const Tensor& src, const float scale, + const float offset, cudaStream_t stream = 0); /** * Batch normalization for one channel interleaved float type Tensor. @@ -851,8 +889,8 @@ void Normalize(Tensor &dst, const Tensor &src, const * @param offset offset constant for normalization. * @stream specified cuda stream. */ -void Normalize(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream = 0); +bool Normalize(Tensor& dst, const Tensor& src, const float scale, + const float offset, cudaStream_t stream = 0); /** * Normalization for one channel planar uint8_t type Tensor. @@ -863,8 +901,8 @@ void Normalize(Tensor &dst, const Tensor &src, con * @param offset offset constant for normalization. * @stream specified cuda stream. */ -void Normalize(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream = 0); +bool Normalize(Tensor& dst, const Tensor& src, const float scale, + const float offset, cudaStream_t stream = 0); /** * Batch normalization for one channel planar uint8_t type Tensor. @@ -875,8 +913,8 @@ void Normalize(Tensor &dst, const Tensor &src, const * @param offset offset constant for normalization. * @stream specified cuda stream. */ -void Normalize(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream = 0); +bool Normalize(Tensor& dst, const Tensor& src, const float scale, + const float offset, cudaStream_t stream = 0); /** * Normalization for one channel planar uint16_t type Tensor. @@ -887,8 +925,8 @@ void Normalize(Tensor &dst, const Tensor &src, cons * @param offset offset constant for normalization. * @stream specified cuda stream. */ -void Normalize(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream = 0); +bool Normalize(Tensor& dst, const Tensor& src, const float scale, + const float offset, cudaStream_t stream = 0); /** * Batch normalization for one channel planar uint16_t type Tensor. @@ -899,8 +937,8 @@ void Normalize(Tensor &dst, const Tensor &src, const * @param offset offset constant for normalization. * @stream specified cuda stream. */ -void Normalize(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream = 0); +bool Normalize(Tensor& dst, const Tensor& src, const float scale, + const float offset, cudaStream_t stream = 0); /** * Normalization for one channel planar float type Tensor. @@ -911,8 +949,8 @@ void Normalize(Tensor &dst, const Tensor &src, con * @param offset offset constant for normalization. * @stream specified cuda stream. */ -void Normalize(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream = 0); +bool Normalize(Tensor& dst, const Tensor& src, const float scale, + const float offset, cudaStream_t stream = 0); /** * Batch normalization for one channel planar float type Tensor. @@ -923,8 +961,8 @@ void Normalize(Tensor &dst, const Tensor &src, const * @param offset offset constant for normalization. * @stream specified cuda stream. */ -void Normalize(Tensor &dst, const Tensor &src, const float scale, const float offset, - cudaStream_t stream = 0); +bool Normalize(Tensor& dst, const Tensor& src, const float scale, + const float offset, cudaStream_t stream = 0); /** * Convert interleaved image to planar image for three channels uint8_t type Tensor. @@ -932,7 +970,8 @@ void Normalize(Tensor &dst, const Tensor &src, con * @param src source tensor. * @param stream specified cuda stream. */ -void InterleavedToPlanar(Tensor &dst, const Tensor &src, cudaStream_t stream = 0); +bool InterleavedToPlanar(Tensor& dst, const Tensor& src, + cudaStream_t stream = 0); /** * Batch convert interleaved image to planar image for three channels uint8_t type Tensor. @@ -940,7 +979,8 @@ void InterleavedToPlanar(Tensor &dst, const Tensor &sr * @param src source tensor. * @param stream specified cuda stream. */ -void InterleavedToPlanar(Tensor &dst, const Tensor &src, cudaStream_t stream = 0); +bool InterleavedToPlanar(Tensor& dst, const Tensor& src, + cudaStream_t stream = 0); /** * Convert interleaved image to planar image for three channels uint16_t type Tensor. @@ -948,7 +988,8 @@ void InterleavedToPlanar(Tensor &dst, const Tensor & * @param src source tensor. * @param stream specified cuda stream. */ -void InterleavedToPlanar(Tensor &dst, const Tensor &src, cudaStream_t stream = 0); +bool InterleavedToPlanar(Tensor& dst, const Tensor& src, + cudaStream_t stream = 0); /** * Batch Convert interleaved image to planar image for three channels uint16_t type Tensor. @@ -956,7 +997,8 @@ void InterleavedToPlanar(Tensor &dst, const Tensor & * @param src source tensor. * @param stream specified cuda stream. */ -void InterleavedToPlanar(Tensor &dst, const Tensor &src, cudaStream_t stream = 0); +bool InterleavedToPlanar(Tensor& dst, const Tensor& src, + cudaStream_t stream = 0); /** * Convert interleaved image to planar image for three channels float type Tensor. @@ -964,7 +1006,8 @@ void InterleavedToPlanar(Tensor &dst, const Tensor * @param src source tensor. * @param stream specified cuda stream. */ -void InterleavedToPlanar(Tensor &dst, const Tensor &src, cudaStream_t stream = 0); +bool InterleavedToPlanar(Tensor& dst, const Tensor& src, + cudaStream_t stream = 0); /** * Batch convert interleaved image to planar image for three channels float type Tensor. @@ -972,7 +1015,8 @@ void InterleavedToPlanar(Tensor &dst, const Tensor & * @param src source tensor. * @param stream specified cuda stream. */ -void InterleavedToPlanar(Tensor &dst, const Tensor &src, cudaStream_t stream = 0); +bool InterleavedToPlanar(Tensor& dst, const Tensor& src, + cudaStream_t stream = 0); /** * Convert interleaved image to planar image for single channel uint8_t type Tensor. @@ -980,7 +1024,8 @@ void InterleavedToPlanar(Tensor &dst, const Tensor * @param src source tensor. * @param stream specified cuda stream. */ -void InterleavedToPlanar(Tensor &dst, const Tensor &src, cudaStream_t stream = 0); +bool InterleavedToPlanar(Tensor& dst, const Tensor& src, + cudaStream_t stream = 0); /** * Batch convert interleaved image to planar image for single channel uint8_t type Tensor. @@ -988,7 +1033,8 @@ void InterleavedToPlanar(Tensor &dst, const Tensor &sr * @param src source tensor. * @param stream specified cuda stream. */ -void InterleavedToPlanar(Tensor &dst, const Tensor &src, cudaStream_t stream = 0); +bool InterleavedToPlanar(Tensor& dst, const Tensor& src, + cudaStream_t stream = 0); /** * Convert interleaved image to planar image for single channel uint16_t type Tensor. @@ -996,7 +1042,8 @@ void InterleavedToPlanar(Tensor &dst, const Tensor & * @param src source tensor. * @param stream specified cuda stream. */ -void InterleavedToPlanar(Tensor &dst, const Tensor &src, cudaStream_t stream = 0); +bool InterleavedToPlanar(Tensor& dst, const Tensor& src, + cudaStream_t stream = 0); /** * Batch Convert interleaved image to planar image for single channel uint16_t type Tensor. @@ -1004,7 +1051,8 @@ void InterleavedToPlanar(Tensor &dst, const Tensor & * @param src source tensor. * @param stream specified cuda stream. */ -void InterleavedToPlanar(Tensor &dst, const Tensor &src, cudaStream_t stream = 0); +bool InterleavedToPlanar(Tensor& dst, const Tensor& src, + cudaStream_t stream = 0); /** * Convert interleaved image to planar image for single channel float type Tensor. @@ -1012,7 +1060,8 @@ void InterleavedToPlanar(Tensor &dst, const Tensor * @param src source tensor. * @param stream specified cuda stream. */ -void InterleavedToPlanar(Tensor &dst, const Tensor &src, cudaStream_t stream = 0); +bool InterleavedToPlanar(Tensor& dst, const Tensor& src, + cudaStream_t stream = 0); /** * Batch convert interleaved image to planar image for single channel float type Tensor. @@ -1020,7 +1069,8 @@ void InterleavedToPlanar(Tensor &dst, const Tensor & * @param src source tensor. * @param stream specified cuda stream. */ -void InterleavedToPlanar(Tensor &dst, const Tensor &src, cudaStream_t stream = 0); +bool InterleavedToPlanar(Tensor& dst, const Tensor& src, + cudaStream_t stream = 0); /** * Combines various functions to imitate OpenCV blobFromImage() for various type tensor input. @@ -1030,9 +1080,8 @@ void InterleavedToPlanar(Tensor &dst, const Tensor * @tparam CT channel type for input tensor (output is fixed: F32). */ template -class ImageToNormalizedPlanarTensorOperator -{ -public: +class ImageToNormalizedPlanarTensorOperator { + public: /** * Implementation for ImageToNormalizedPlanarTensorOperator. */ @@ -1066,8 +1115,9 @@ class ImageToNormalizedPlanarTensorOperator * @param stream specified cuda stream. */ template::type * = nullptr> - void operator()(Tensor &dst, const Tensor &src, const float scale[3], - const float offset[3], bool swapRB, bool keep_aspect_ratio = false, cudaStream_t stream = 0); + void operator()(Tensor& dst, const Tensor& src, + const float scale[3], const float offset[3], bool swapRB, + bool keep_aspect_ratio = false, cudaStream_t stream = 0); /** * Run the composite operations on single channel tensors. @@ -1079,13 +1129,12 @@ class ImageToNormalizedPlanarTensorOperator * @param stream specified cuda stream. */ template::type * = nullptr> - void operator()(Tensor &dst, const Tensor &src, float scale, float offset, - bool keep_aspect_ratio = false, cudaStream_t stream = 0); + void operator()(Tensor& dst, const Tensor& src, + float scale, float offset, bool keep_aspect_ratio = false, cudaStream_t stream = 0); -private: + private: std::unique_ptr m_pImpl; }; -}} // namespace cvcore::tensor_ops - -#endif // CVCORE_IMAGE_UTILS_H +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/core/cvcore/MathTypes.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/MathTypes.cpp similarity index 91% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/src/core/cvcore/MathTypes.cpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/MathTypes.cpp index e0b4adc..d5e5ee3 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/core/cvcore/MathTypes.cpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/MathTypes.cpp @@ -15,14 +15,13 @@ // // SPDX-License-Identifier: Apache-2.0 -#include "cv/core/MathTypes.h" +#include "extensions/tensorops/core/MathTypes.h" namespace cvcore { - +namespace tensor_ops { namespace { -AxisAngleRotation RotationMatrixToAxisAngleUtil(const std::vector &rotMatrix) -{ +AxisAngleRotation RotationMatrixToAxisAngleUtil(const std::vector & rotMatrix) { assert(rotMatrix.size() == 9); AxisAngleRotation axisangle; double row0 = 0.5 * (rotMatrix[7] - rotMatrix[5]); @@ -34,15 +33,12 @@ AxisAngleRotation RotationMatrixToAxisAngleUtil(const std::vector &rotMa sin_angle = sin_angle > 1.0 ? 1.0 : sin_angle; cos_angle = cos_angle > 1.0 ? 1.0 : (cos_angle < -1.0 ? -1.0 : cos_angle); - if (sin_angle == 0.0) - { + if (sin_angle == 0.0) { axisangle.angle = 0; axisangle.axis.x = 0; axisangle.axis.y = 0; axisangle.axis.z = 0; - } - else - { + } else { axisangle.angle = std::atan2(sin_angle, cos_angle); axisangle.axis.x = row0 / sin_angle; axisangle.axis.y = row1 / sin_angle; @@ -50,10 +46,9 @@ AxisAngleRotation RotationMatrixToAxisAngleUtil(const std::vector &rotMa } return axisangle; } -} // namespace +} // namespace -Vector3d RotationMatrixToRotationVector(const std::vector &rotMatrix) -{ +Vector3d RotationMatrixToRotationVector(const std::vector & rotMatrix) { AxisAngleRotation axisangle = RotationMatrixToAxisAngleUtil(rotMatrix); Vector3d rotVector; rotVector.x = axisangle.angle * axisangle.axis.x; @@ -62,14 +57,12 @@ Vector3d RotationMatrixToRotationVector(const std::vector &rotMatrix) return rotVector; } -AxisAngleRotation RotationMatrixToAxisAngleRotation(const std::vector &rotMatrix) -{ +AxisAngleRotation RotationMatrixToAxisAngleRotation(const std::vector & rotMatrix) { AxisAngleRotation axisangle = RotationMatrixToAxisAngleUtil(rotMatrix); return axisangle; } -std::vector AxisAngleToRotationMatrix(const AxisAngleRotation &axisangle) -{ +std::vector AxisAngleToRotationMatrix(const AxisAngleRotation & axisangle) { std::vector rotMatrix; rotMatrix.resize(9); double cosangle = std::cos(axisangle.angle); @@ -95,8 +88,7 @@ std::vector AxisAngleToRotationMatrix(const AxisAngleRotation &axisangle return rotMatrix; } -Vector3d AxisAngleRotationToRotationVector(const AxisAngleRotation &axisangle) -{ +Vector3d AxisAngleRotationToRotationVector(const AxisAngleRotation & axisangle) { double angle = axisangle.angle; Vector3d rotVector; rotVector.x = angle * axisangle.axis.x; @@ -105,20 +97,17 @@ Vector3d AxisAngleRotationToRotationVector(const AxisAngleRotation &axisangle) return rotVector; } -AxisAngleRotation RotationVectorToAxisAngleRotation(const Vector3d &rotVector) -{ - double normVector = rotVector.x * rotVector.x + rotVector.y * rotVector.y + rotVector.z * rotVector.z; +AxisAngleRotation RotationVectorToAxisAngleRotation(const Vector3d & rotVector) { + double normVector = rotVector.x * rotVector.x + rotVector.y * rotVector.y + + rotVector.z * rotVector.z; normVector = std::sqrt(normVector); AxisAngleRotation axisangle; - if (normVector == 0) - { + if (normVector == 0) { axisangle.angle = 0; axisangle.axis.x = 0; axisangle.axis.y = 0; axisangle.axis.z = 0; - } - else - { + } else { axisangle.angle = normVector; axisangle.axis.x = rotVector.x / normVector; axisangle.axis.y = rotVector.y / normVector; @@ -127,8 +116,7 @@ AxisAngleRotation RotationVectorToAxisAngleRotation(const Vector3d &rotVector) return axisangle; } -Quaternion AxisAngleRotationToQuaternion(const AxisAngleRotation &axisangle) -{ +Quaternion AxisAngleRotationToQuaternion(const AxisAngleRotation & axisangle) { Quaternion qrotation; qrotation.qx = axisangle.axis.x * sin(axisangle.angle / 2); qrotation.qy = axisangle.axis.y * sin(axisangle.angle / 2); @@ -137,13 +125,11 @@ Quaternion AxisAngleRotationToQuaternion(const AxisAngleRotation &axisangle) return qrotation; } -AxisAngleRotation QuaternionToAxisAngleRotation(const Quaternion &qrotation) -{ +AxisAngleRotation QuaternionToAxisAngleRotation(const Quaternion & qrotation) { Quaternion qtemp(qrotation.qx, qrotation.qy, qrotation.qz, qrotation.qw); - if (qrotation.qw > 1) - { - double qnorm = qrotation.qx * qrotation.qx + qrotation.qy * qrotation.qy + qrotation.qz * qrotation.qz + - qrotation.qw * qrotation.qw; + if (qrotation.qw > 1) { + double qnorm = qrotation.qx * qrotation.qx + qrotation.qy * qrotation.qy + + qrotation.qz * qrotation.qz + qrotation.qw * qrotation.qw; qnorm = std::sqrt(qnorm); qtemp.qx = qrotation.qx / qnorm; qtemp.qy = qrotation.qy / qnorm; @@ -153,14 +139,11 @@ AxisAngleRotation QuaternionToAxisAngleRotation(const Quaternion &qrotation) AxisAngleRotation axisangle; axisangle.angle = 2 * std::acos(qtemp.qw); double normaxis = std::sqrt(1 - qtemp.qw * qtemp.qw); - if (normaxis < 0.001) - { + if (normaxis < 0.001) { axisangle.axis.x = qtemp.qx; axisangle.axis.y = qtemp.qy; axisangle.axis.z = qtemp.qz; - } - else - { + } else { axisangle.axis.x = qtemp.qx / normaxis; axisangle.axis.y = qtemp.qy / normaxis; axisangle.axis.z = qtemp.qz / normaxis; @@ -168,8 +151,7 @@ AxisAngleRotation QuaternionToAxisAngleRotation(const Quaternion &qrotation) return axisangle; } -std::vector QuaternionToRotationMatrix(const Quaternion &qrotation) -{ +std::vector QuaternionToRotationMatrix(const Quaternion & qrotation) { std::vector rotMatrix; rotMatrix.resize(9); double qxsquare = qrotation.qx * qrotation.qx; @@ -199,38 +181,29 @@ std::vector QuaternionToRotationMatrix(const Quaternion &qrotation) return rotMatrix; } -Quaternion RotationMatrixToQuaternion(const std::vector &rotMatrix) -{ +Quaternion RotationMatrixToQuaternion(const std::vector & rotMatrix) { Quaternion qrotation; double diagsum = rotMatrix[0] + rotMatrix[4] + rotMatrix[8]; - if (diagsum > 0) - { + if (diagsum > 0) { double temp = 1 / (2 * std::sqrt(diagsum + 1.0)); qrotation.qw = 0.25 / temp; qrotation.qx = (rotMatrix[7] - rotMatrix[5]) * temp; qrotation.qy = (rotMatrix[2] - rotMatrix[6]) * temp; qrotation.qz = (rotMatrix[3] - rotMatrix[1]) * temp; - } - else - { - if (rotMatrix[0] > rotMatrix[4] && rotMatrix[0] > rotMatrix[8]) - { + } else { + if (rotMatrix[0] > rotMatrix[4] && rotMatrix[0] > rotMatrix[8]) { double temp = 2 * std::sqrt(rotMatrix[0] - rotMatrix[4] - rotMatrix[8] + 1.0); qrotation.qx = 0.25 * temp; qrotation.qw = (rotMatrix[7] - rotMatrix[5]) * temp; qrotation.qy = (rotMatrix[2] - rotMatrix[6]) * temp; qrotation.qz = (rotMatrix[3] - rotMatrix[1]) * temp; - } - else if (rotMatrix[0] > rotMatrix[8]) - { + } else if (rotMatrix[0] > rotMatrix[8]) { double temp = 2 * std::sqrt(rotMatrix[4] - rotMatrix[0] - rotMatrix[8] + 1.0); qrotation.qy = 0.25 * temp; qrotation.qx = (rotMatrix[7] - rotMatrix[5]) * temp; qrotation.qw = (rotMatrix[2] - rotMatrix[6]) * temp; qrotation.qz = (rotMatrix[3] - rotMatrix[1]) * temp; - } - else - { + } else { double temp = 2 * std::sqrt(rotMatrix[8] - rotMatrix[4] - rotMatrix[0] + 1.0); qrotation.qz = 0.25 * temp; qrotation.qx = (rotMatrix[7] - rotMatrix[5]) * temp; @@ -241,4 +214,5 @@ Quaternion RotationMatrixToQuaternion(const std::vector &rotMatrix) return qrotation; } -} // namespace cvcore +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/MathTypes.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/MathTypes.h similarity index 86% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/MathTypes.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/MathTypes.h index fd9db1a..a7a0c8a 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/MathTypes.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/MathTypes.h @@ -15,8 +15,7 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_Math_H -#define CVCORE_Math_H +#pragma once #include #include @@ -26,6 +25,7 @@ #include "Tensor.h" namespace cvcore { +namespace tensor_ops { /** Matrix types using Tensor backend */ using Matrixf = Tensor; @@ -36,23 +36,16 @@ using Matrixd = Tensor; * Structure used to store Vector2 Values. */ template -struct Vector2 -{ +struct Vector2 { T x; /**< point x coordinate. */ T y; /**< point y coordinate. */ - inline T &operator[](size_t i) - { - if (i == 0) - { + inline T & operator[](size_t i) { + if (i == 0) { return x; - } - else if (i == 1) - { + } else if (i == 1) { return y; - } - else - { + } else { throw std::out_of_range("cvcore::Vector2 ==> Requested index is out of bounds"); } } @@ -63,28 +56,19 @@ struct Vector2 * Structure used to store Vector3 Values. */ template -struct Vector3 -{ +struct Vector3 { T x; /**< point x coordinate. */ T y; /**< point y coordinate. */ T z; /**< point z coordinate. */ - inline T &operator[](size_t i) - { - if (i == 0) - { + inline T & operator[](size_t i) { + if (i == 0) { return x; - } - else if (i == 1) - { + } else if (i == 1) { return y; - } - else if (i == 2) - { + } else if (i == 2) { return z; - } - else - { + } else { throw std::out_of_range("cvcore::Vector3 ==> Requested index is out of bounds"); } } @@ -103,21 +87,18 @@ using Vector3d = Vector3; * A struct * Structure used to store AxisAngle Rotation parameters. */ -struct AxisAngleRotation -{ +struct AxisAngleRotation { double angle; /** Counterclockwise rotation angle [0, 2PI]. */ Vector3d axis; /** 3d axis of rotation. */ AxisAngleRotation() : angle(0.0) - , axis{0, 0, 0} - { + , axis{0, 0, 0} { } AxisAngleRotation(double angleinput, Vector3d axisinput) : angle(angleinput) - , axis(axisinput) - { + , axis(axisinput) { } }; @@ -127,8 +108,7 @@ struct AxisAngleRotation * A rotation of unit vector u with rotation theta can be represented in quaternion as: * q={cos(theta/2)+ i(u*sin(theta/2))} */ -struct Quaternion -{ +struct Quaternion { double qx, qy, qz; /** Axis or imaginary component of the quaternion representation. */ double qw; /** Angle or real component of the quaternion representation. */ @@ -136,16 +116,14 @@ struct Quaternion : qx(0.0) , qy(0.0) , qz(0.0) - , qw(0.0) - { + , qw(0.0) { } Quaternion(double qxinput, double qyinput, double qzinput, double qwinput) : qx(qxinput) , qy(qyinput) , qz(qzinput) - , qw(qwinput) - { + , qw(qwinput) { } }; @@ -155,21 +133,21 @@ struct Quaternion * @return 3D Rotation vector {theta * xaxis, theta * yaxis, theta * zaxis} * where theta is the angle of rotation in radians */ -Vector3d RotationMatrixToRotationVector(const std::vector &rotMatrix); +Vector3d RotationMatrixToRotationVector(const std::vector & rotMatrix); /** * Convert rotation matrix to axis angle representation. * @param rotMatrix Rotation matrix of 3x3 values. * @return Axis angle rotation */ -AxisAngleRotation RotationMatrixToAxisAngleRotation(const std::vector &rotMatrix); +AxisAngleRotation RotationMatrixToAxisAngleRotation(const std::vector & rotMatrix); /** * Convert axis angle representation to rotation matrix. * @param axisangle Axis angle rotation. * @return Rotation matrix of 3x3 values. */ -std::vector AxisAngleToRotationMatrix(const AxisAngleRotation &axisangle); +std::vector AxisAngleToRotationMatrix(const AxisAngleRotation & axisangle); /** * Convert axis angle representation to 3d rotation vector. @@ -178,50 +156,49 @@ std::vector AxisAngleToRotationMatrix(const AxisAngleRotation &axisangle * @param axisangle Axis angle rotation. * @return 3D Rotation Vector */ -Vector3d AxisAngleRotationToRotationVector(const AxisAngleRotation &axisangle); +Vector3d AxisAngleRotationToRotationVector(const AxisAngleRotation & axisangle); /** * Convert rotation vector to axis angle representation. * @param rotVector 3D rotation vector. * @return Axis angle rotation. */ -AxisAngleRotation RotationVectorToAxisAngleRotation(const Vector3d &rotVector); +AxisAngleRotation RotationVectorToAxisAngleRotation(const Vector3d & rotVector); /** * Convert axis angle representation to quaternion. * @param axisangle Axis angle representation. * @return Quaternion rotation. */ -Quaternion AxisAngleRotationToQuaternion(const AxisAngleRotation &axisangle); +Quaternion AxisAngleRotationToQuaternion(const AxisAngleRotation & axisangle); /** * Convert quaternion rotation to axis angle rotation. * @param qrotation Quaternion rotation representation. * @return Axis angle rotation. */ -AxisAngleRotation QuaternionToAxisAngleRotation(const Quaternion &qrotation); +AxisAngleRotation QuaternionToAxisAngleRotation(const Quaternion & qrotation); /** * Convert quaternion rotation to rotation matrix. * @param qrotation Quaternion rotation representation. * @return Rotation matrix. */ -std::vector QuaternionToRotationMatrix(const Quaternion &qrotation); +std::vector QuaternionToRotationMatrix(const Quaternion & qrotation); /** * Convert rotation matrix to Quaternion. * @param rotMatrix Rotation matrix * @return Quaternion rotation. */ -Quaternion RotationMatrixToQuaternion(const std::vector &rotMatrix); +Quaternion RotationMatrixToQuaternion(const std::vector & rotMatrix); /** * A struct. * Structure used to store Pose3D parameters. */ template -struct Pose3 -{ +struct Pose3 { AxisAngleRotation rotation; /**Rotation expressed in axis angle notation.*/ Vector3 translation; /*Translation expressed as x,y,z coordinates.*/ }; @@ -229,6 +206,5 @@ struct Pose3 using Pose3d = Pose3; using Pose3f = Pose3; -} // namespace cvcore - -#endif // CVCORE_Math_H +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/core/utility/Memory.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Memory.cpp similarity index 58% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/src/core/utility/Memory.cpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Memory.cpp index e75a614..141800b 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/core/utility/Memory.cpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Memory.cpp @@ -15,7 +15,7 @@ // // SPDX-License-Identifier: Apache-2.0 -#include "cv/core/Memory.h" +#include "extensions/tensorops/core/Memory.h" #include @@ -24,101 +24,83 @@ #include namespace cvcore { - +namespace tensor_ops { namespace { // Copy 2D CPU pitch linear tensors -void Memcpy2DCPU(void *dst, size_t dstPitch, const void *src, size_t srcPitch, size_t widthInBytes, size_t height) -{ - uint8_t *dstPt = reinterpret_cast(dst); - const uint8_t *srcPt = reinterpret_cast(src); - for (size_t i = 0; i < height; i++) - { +void Memcpy2DCPU(void * dst, size_t dstPitch, const void * src, size_t srcPitch, + size_t widthInBytes, size_t height) { + uint8_t* dstPt = reinterpret_cast(dst); + const uint8_t* srcPt = reinterpret_cast(src); + for (size_t i = 0; i < height; i++) { memcpy(dstPt, srcPt, widthInBytes); dstPt += dstPitch; srcPt += srcPitch; } } -} // anonymous namespace +} // anonymous namespace -void TensorBaseCopy(TensorBase &dst, const TensorBase &src, cudaStream_t stream) -{ - if (dst.getDataSize() != src.getDataSize()) - { +void TensorBaseCopy(TensorBase & dst, const TensorBase & src, cudaStream_t stream) { + if (dst.getDataSize() != src.getDataSize()) { throw std::runtime_error("Tensor stride mismatch!"); } assert(dst.getDimCount() == src.getDimCount()); int dimCount = src.getDimCount(); - for (int i = 0; i < dimCount - 1; i++) - { + for (int i = 0; i < dimCount - 1; i++) { if (src.getStride(i) != src.getStride(i + 1) * src.getSize(i + 1) || - dst.getStride(i) != dst.getStride(i + 1) * dst.getSize(i + 1)) - { + dst.getStride(i) != dst.getStride(i + 1) * dst.getSize(i + 1)) { throw std::runtime_error("Tensor is not contiguous in memory!"); } } - if (dst.isCPU() && src.isCPU()) - { + if (dst.isCPU() && src.isCPU()) { memcpy(dst.getData(), src.getData(), src.getDataSize()); return; } cudaError_t error; - if (!dst.isCPU() && src.isCPU()) - { - error = cudaMemcpyAsync(dst.getData(), src.getData(), src.getDataSize(), cudaMemcpyHostToDevice, stream); - } - else if (dst.isCPU() && !src.isCPU()) - { - error = cudaMemcpyAsync(dst.getData(), src.getData(), src.getDataSize(), cudaMemcpyDeviceToHost, stream); - } - else - { - error = cudaMemcpyAsync(dst.getData(), src.getData(), src.getDataSize(), cudaMemcpyDeviceToDevice, stream); + if (!dst.isCPU() && src.isCPU()) { + error = cudaMemcpyAsync(dst.getData(), src.getData(), src.getDataSize(), + cudaMemcpyHostToDevice, stream); + } else if (dst.isCPU() && !src.isCPU()) { + error = cudaMemcpyAsync(dst.getData(), src.getData(), src.getDataSize(), + cudaMemcpyDeviceToHost, stream); + } else { + error = cudaMemcpyAsync(dst.getData(), src.getData(), src.getDataSize(), + cudaMemcpyDeviceToDevice, stream); } - if (error != cudaSuccess) - { + if (error != cudaSuccess) { throw std::runtime_error("CUDA memcpy failed!"); } } -void TensorBaseCopy2D(TensorBase &dst, const TensorBase &src, int dstPitch, int srcPitch, int widthInBytes, int height, - cudaStream_t stream) -{ +void TensorBaseCopy2D(TensorBase & dst, const TensorBase & src, int dstPitch, + int srcPitch, int widthInBytes, int height, cudaStream_t stream) { assert(dst.getDimCount() == src.getDimCount()); int dimCount = src.getDimCount(); - for (int i = 0; i < dimCount; i++) - { - if (dst.getSize(i) != src.getSize(i)) - { + for (int i = 0; i < dimCount; i++) { + if (dst.getSize(i) != src.getSize(i)) { throw std::runtime_error("Tensor size mismatch!"); } } - if (dst.isCPU() && src.isCPU()) - { + if (dst.isCPU() && src.isCPU()) { Memcpy2DCPU(dst.getData(), dstPitch, src.getData(), srcPitch, widthInBytes, height); return; } cudaError_t error; - if (!dst.isCPU() && src.isCPU()) - { - error = cudaMemcpy2DAsync(dst.getData(), dstPitch, src.getData(), srcPitch, widthInBytes, height, - cudaMemcpyHostToDevice, stream); - } - else if (dst.isCPU() && !src.isCPU()) - { - error = cudaMemcpy2DAsync(dst.getData(), dstPitch, src.getData(), srcPitch, widthInBytes, height, - cudaMemcpyDeviceToHost, stream); - } - else - { - error = cudaMemcpy2DAsync(dst.getData(), dstPitch, src.getData(), srcPitch, widthInBytes, height, - cudaMemcpyDeviceToDevice, stream); + if (!dst.isCPU() && src.isCPU()) { + error = cudaMemcpy2DAsync(dst.getData(), dstPitch, src.getData(), srcPitch, + widthInBytes, height, cudaMemcpyHostToDevice, stream); + } else if (dst.isCPU() && !src.isCPU()) { + error = cudaMemcpy2DAsync(dst.getData(), dstPitch, src.getData(), srcPitch, + widthInBytes, height, cudaMemcpyDeviceToHost, stream); + } else { + error = cudaMemcpy2DAsync(dst.getData(), dstPitch, src.getData(), srcPitch, + widthInBytes, height, cudaMemcpyDeviceToDevice, stream); } - if (error != cudaSuccess) - { + if (error != cudaSuccess) { throw std::runtime_error("CUDA memcpy failed!"); } } -} // namespace cvcore +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/Memory.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Memory.h similarity index 58% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/Memory.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Memory.h index 7d3d113..f8d79a8 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/Memory.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Memory.h @@ -15,14 +15,14 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_MEMORY_H -#define CVCORE_MEMORY_H +#pragma once #include -#include "Tensor.h" +#include "extensions/tensorops/core/Tensor.h" namespace cvcore { +namespace tensor_ops { /** * Implementation of tensor copy. @@ -30,7 +30,7 @@ namespace cvcore { * @param src source TensorBase. * @param stream cuda stream. */ -void TensorBaseCopy(TensorBase &dst, const TensorBase &src, cudaStream_t stream = 0); +void TensorBaseCopy(TensorBase & dst, const TensorBase & src, cudaStream_t stream = 0); /** * Implementation of tensor copy for 2D pitch linear tensors. @@ -42,8 +42,8 @@ void TensorBaseCopy(TensorBase &dst, const TensorBase &src, cudaStream_t stream * @param height height of tensor. * @param stream cuda stream. */ -void TensorBaseCopy2D(TensorBase &dst, const TensorBase &src, int dstPitch, int srcPitch, int widthInBytes, int height, - cudaStream_t stream = 0); +void TensorBaseCopy2D(TensorBase & dst, const TensorBase& src, int dstPitch, + int srcPitch, int widthInBytes, int height, cudaStream_t stream = 0); /** * Memory copy function between two non HWC/CHW/NHWC/NCHW Tensors. @@ -55,9 +55,9 @@ void TensorBaseCopy2D(TensorBase &dst, const TensorBase &src, int dstPitch, int * @param stream cuda stream. */ template::type * = nullptr> -void Copy(Tensor &dst, const Tensor &src, cudaStream_t stream = 0) -{ + typename std::enable_if::type * = nullptr> +void Copy(Tensor& dst, const Tensor& src, cudaStream_t stream = 0) { TensorBaseCopy(dst, src, stream); } @@ -70,12 +70,13 @@ void Copy(Tensor &dst, const Tensor &src, cudaStream_t s * @param src source Tensor which copy from. * @param stream cuda stream. */ -template::type * = nullptr> -void Copy(Tensor &dst, const Tensor &src, cudaStream_t stream = 0) -{ +template::type * = nullptr> +void Copy(Tensor& dst, const Tensor& src, + cudaStream_t stream = 0) { TensorBaseCopy2D(dst, src, dst.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), - src.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), - dst.getWidth() * dst.getChannelCount() * GetChannelSize(CT), src.getHeight(), stream); + src.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), + dst.getWidth() * dst.getChannelCount() * GetChannelSize(CT), src.getHeight(), stream); } /** @@ -87,13 +88,13 @@ void Copy(Tensor &dst, const Tensor &src, cudaStream_t s * @param src source Tensor which copy from. * @param stream cuda stream. */ -template::type * = nullptr> -void Copy(Tensor &dst, const Tensor &src, cudaStream_t stream = 0) -{ +template::type * = nullptr> +void Copy(Tensor& dst, const Tensor& src, cudaStream_t stream = 0) { TensorBaseCopy2D(dst, src, dst.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), - src.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), - dst.getWidth() * dst.getChannelCount() * GetChannelSize(CT), src.getDepth() * src.getHeight(), - stream); + src.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), + dst.getWidth() * dst.getChannelCount() * GetChannelSize(CT), + src.getDepth() * src.getHeight(), stream); } /** @@ -105,12 +106,13 @@ void Copy(Tensor &dst, const Tensor &src, cudaStream_t s * @param src source Tensor which copy from. * @param stream cuda stream. */ -template::type * = nullptr> -void Copy(Tensor &dst, const Tensor &src, cudaStream_t stream = 0) -{ +template::type * = nullptr> +void Copy(Tensor& dst, const Tensor& src, cudaStream_t stream = 0) { TensorBaseCopy2D(dst, src, dst.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), - src.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), dst.getWidth() * GetChannelSize(CT), - src.getChannelCount() * src.getHeight(), stream); + src.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), + dst.getWidth() * GetChannelSize(CT), + src.getChannelCount() * src.getHeight(), stream); } /** @@ -122,14 +124,14 @@ void Copy(Tensor &dst, const Tensor &src, cudaStream_t s * @param src source Tensor which copy from. * @param stream cuda stream. */ -template::type * = nullptr> -void Copy(Tensor &dst, const Tensor &src, cudaStream_t stream = 0) -{ +template::type * = nullptr> +void Copy(Tensor& dst, const Tensor& src, cudaStream_t stream = 0) { TensorBaseCopy2D(dst, src, dst.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), - src.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), dst.getWidth() * GetChannelSize(CT), - src.getDepth() * src.getChannelCount() * src.getHeight(), stream); + src.getStride(TensorDimension::HEIGHT) * GetChannelSize(CT), + dst.getWidth() * GetChannelSize(CT), + src.getDepth() * src.getChannelCount() * src.getHeight(), stream); } -} // namespace cvcore - -#endif // CVCORE_MEMORY_H +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/NppUtils.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/NppUtils.cpp similarity index 75% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/NppUtils.cpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/NppUtils.cpp index 4c7cf7f..23d9275 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/NppUtils.cpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/NppUtils.cpp @@ -15,14 +15,15 @@ // // SPDX-License-Identifier: Apache-2.0 -#include "NppUtils.h" +#include "extensions/tensorops/core/NppUtils.h" #include #include #include #include -namespace cvcore { namespace tensor_ops { +namespace cvcore { +namespace tensor_ops { constexpr size_t CACHE_SIZE = 20; static size_t timestamp = 0; @@ -31,24 +32,20 @@ std::mutex lock; namespace { // This function involves GPU query and can be really slow -void SetupNppStreamContext(NppStreamContext &context, cudaStream_t stream) -{ +void SetupNppStreamContext(NppStreamContext& context, cudaStream_t stream) { context.hStream = stream; cudaError_t error = cudaGetDevice(&context.nCudaDeviceId); - if (error != cudaSuccess) - { + if (error != cudaSuccess) { throw std::runtime_error("no devices supporting CUDA"); } error = cudaStreamGetFlags(context.hStream, &context.nStreamFlags); - if (error != cudaSuccess) - { + if (error != cudaSuccess) { throw std::runtime_error("failed to get cuda stream flags"); } cudaDeviceProp deviceProp; error = cudaGetDeviceProperties(&deviceProp, context.nCudaDeviceId); - if (error != cudaSuccess) - { + if (error != cudaSuccess) { throw std::runtime_error("no device properties"); } @@ -60,31 +57,29 @@ void SetupNppStreamContext(NppStreamContext &context, cudaStream_t stream) // Refer - https://gitlab-master.nvidia.com/cv/core-modules/tensor_ops/-/merge_requests/48#note_6602087 context.nReserved0 = 0; - error = cudaDeviceGetAttribute(&(context.nCudaDevAttrComputeCapabilityMajor), cudaDevAttrComputeCapabilityMajor, - context.nCudaDeviceId); - if (error != cudaSuccess) - { + error = cudaDeviceGetAttribute(&(context.nCudaDevAttrComputeCapabilityMajor), + cudaDevAttrComputeCapabilityMajor, + context.nCudaDeviceId); + if (error != cudaSuccess) { throw std::runtime_error("no device attribute - nCudaDevAttrComputeCapabilityMajor"); } - error = cudaDeviceGetAttribute(&(context.nCudaDevAttrComputeCapabilityMinor), cudaDevAttrComputeCapabilityMinor, - context.nCudaDeviceId); - if (error != cudaSuccess) - { + error = cudaDeviceGetAttribute(&(context.nCudaDevAttrComputeCapabilityMinor), + cudaDevAttrComputeCapabilityMinor, + context.nCudaDeviceId); + if (error != cudaSuccess) { throw std::runtime_error("no device attribute - nCudaDevAttrComputeCapabilityMinor"); } } -} // anonymous namespace +} // anonymous namespace -struct Context -{ +struct Context { NppStreamContext nppContext; size_t time = 0; }; -NppStreamContext GetNppStreamContext(cudaStream_t stream) -{ +NppStreamContext GetNppStreamContext(cudaStream_t stream) { // Create a memory cache, all timestamp would be initialzed to 0 automatically static std::array contextCache = {}; @@ -93,24 +88,22 @@ NppStreamContext GetNppStreamContext(cudaStream_t stream) size_t minTimestamp = contextCache[0].time; size_t minIdx = 0; - for (size_t i = 0; i < CACHE_SIZE; i++) - { - auto &it = contextCache[i]; - if (it.time > 0 && it.nppContext.hStream == stream) - { + for (size_t i = 0; i < CACHE_SIZE; i++) { + auto& it = contextCache[i]; + if (it.time > 0 && it.nppContext.hStream == stream) { it.time = ++timestamp; return it.nppContext; } - if (it.time < minTimestamp) - { + if (it.time < minTimestamp) { minTimestamp = it.time; minIdx = i; } } - auto &it = contextCache[minIdx]; + auto& it = contextCache[minIdx]; SetupNppStreamContext(it.nppContext, stream); it.time = ++timestamp; return it.nppContext; } -}} // namespace cvcore::tensor_ops +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/NppUtils.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/NppUtils.h similarity index 81% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/NppUtils.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/NppUtils.h index 398ef8f..ce4d97e 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/NppUtils.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/NppUtils.h @@ -15,17 +15,15 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_NPP_UTILS_H -#define CVCORE_NPP_UTILS_H - -#include +#pragma once #include +#include "nppdefs.h" // NOLINT -namespace cvcore { namespace tensor_ops { +namespace cvcore { +namespace tensor_ops { NppStreamContext GetNppStreamContext(cudaStream_t stream); -}} // namespace cvcore::tensor_ops - -#endif // CVCORE_NPP_UTILS_H +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/OneEuroFilter.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/OneEuroFilter.cpp similarity index 60% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/OneEuroFilter.cpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/OneEuroFilter.cpp index 35041b8..e3ead12 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/OneEuroFilter.cpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/OneEuroFilter.cpp @@ -15,21 +15,16 @@ // // SPDX-License-Identifier: Apache-2.0 -#include "cv/tensor_ops/OneEuroFilter.h" -#include "cv/core/MathTypes.h" -#include "cv/core/Traits.h" - #include #include #include -#ifdef NVBENCH_ENABLE -#include -#endif - -namespace cvcore { namespace tensor_ops { +#include "extensions/tensorops/core/MathTypes.h" +#include "extensions/tensorops/core/OneEuroFilter.h" +#include "extensions/tensorops/core/Traits.h" -namespace { +namespace cvcore { +namespace tensor_ops { // 1/(2*PI) constexpr float kOneOver2Pi = 0.15915494309189533577f; @@ -38,59 +33,43 @@ constexpr float kOneOver2Pi = 0.15915494309189533577f; template struct deduceDataType; template -struct deduceDataType::value || std::is_same::value || - std::is_same::value>::type> -{ +struct deduceDataType::value || + std::is_same::value || + std::is_same::value>::type> { typedef float U; }; template -struct deduceDataType::value || std::is_same::value || - std::is_same::value>::type> -{ +struct deduceDataType::value || + std::is_same::value || + std::is_same::value>::type> { typedef double U; }; -} // namespace - /* Low pass filter to apply exponential smoothing*/ template -class LowPassfilter -{ -public: - LowPassfilter() - { +class LowPassfilter { + public: + LowPassfilter() { m_firstIteration = true; } - void resetState() - { + void resetState() { m_firstIteration = true; } - bool isInitialized() const - { + bool isInitialized() const { return !m_firstIteration; } - T getPreviousValue() const - { + T getPreviousValue() const { return m_prevfilteredValue; } - std::error_code filter(T &outValue, T inValue, float alpha) - { -#ifdef NVBENCH_ENABLE - std::string funcName = "LowPassFilter_"; - std::string tag = funcName + typeid(T).name(); - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif - if (m_firstIteration) - { + std::error_code filter(T& outValue, T inValue, float alpha) { + if (m_firstIteration) { outValue = inValue; m_firstIteration = false; - } - else - { + } else { outValue = m_prevfilteredValue + (inValue - m_prevfilteredValue) * alpha; } m_prevRawValue = inValue; @@ -98,27 +77,24 @@ class LowPassfilter return ErrorCode::SUCCESS; } -private: + private: bool m_firstIteration; T m_prevRawValue; T m_prevfilteredValue; }; template -struct OneEuroFilterState -{ +struct OneEuroFilterState { // Computes alpha value for the filter - float getAlpha(float dataUpdateRate, float cutOffFreq) const - { + float getAlpha(float dataUpdateRate, float cutOffFreq) const { float alpha = cutOffFreq / (dataUpdateRate * kOneOver2Pi + cutOffFreq); return alpha; } // Resets the parameters and state of the filter - std::error_code resetParams(const OneEuroFilterParams &filterParams) - { - if (filterParams.dataUpdateRate <= 0.0f || filterParams.minCutoffFreq <= 0 || filterParams.derivCutoffFreq <= 0) - { + std::error_code resetParams(const OneEuroFilterParams& filterParams) { + if (filterParams.dataUpdateRate <= 0.0f || filterParams.minCutoffFreq <= 0 || + filterParams.derivCutoffFreq <= 0) { return ErrorCode::INVALID_ARGUMENT; } m_freq = filterParams.dataUpdateRate; @@ -136,38 +112,29 @@ struct OneEuroFilterState } // Constructor for each filter state - OneEuroFilterState(const OneEuroFilterParams &filterParams) - { + OneEuroFilterState(const OneEuroFilterParams& filterParams) { xFilt.reset(new LowPassfilter()); dxFilt.reset(new LowPassfilter()); auto err = resetParams(filterParams); - if (err != make_error_code(ErrorCode::SUCCESS)) - { + if (err != make_error_code(ErrorCode::SUCCESS)) { throw err; } } - std::error_code filter(U &outValue, U value) - { -#ifdef NVBENCH_ENABLE - std::string funcName = "OneEuroFilterState_"; - std::string tag = funcName + typeid(U).name(); - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif + std::error_code filter(U& outValue, U value) { m_prevfilteredValue = m_currfilteredValue; - U dxValue = xFilt->isInitialized() ? (value - xFilt->getPreviousValue()) * m_freq : 0.0f; + U dxValue = xFilt->isInitialized() + ? (value - xFilt->getPreviousValue()) * m_freq : 0.0f; U edxValue; auto err = dxFilt->filter(edxValue, dxValue, m_alphadxFilt); - if (err != make_error_code(ErrorCode::SUCCESS)) - { + if (err != make_error_code(ErrorCode::SUCCESS)) { return err; } // Update the new cutoff frequency U newCutoff = m_mincutoff + m_cutOffSlope * fabsf(edxValue); float newAlpha = getAlpha(m_freq, newCutoff); err = xFilt->filter(m_currfilteredValue, value, newAlpha); - if (err != make_error_code(ErrorCode::SUCCESS)) - { + if (err != make_error_code(ErrorCode::SUCCESS)) { return err; } @@ -186,27 +153,21 @@ struct OneEuroFilterState }; template -struct OneEuroFilter::OneEuroFilterImpl -{ +struct OneEuroFilter::OneEuroFilterImpl { typedef typename deduceDataType::U DT; - OneEuroFilterImpl(const OneEuroFilterParams &filterParams) - { - size_t numStates = traits::get_dim::value; + OneEuroFilterImpl(const OneEuroFilterParams& filterParams) { + size_t numStates = get_dim::value; m_states.resize(numStates); - for (size_t i = 0; i < m_states.size(); i++) - { + for (size_t i = 0; i < m_states.size(); i++) { m_states[i].reset(new OneEuroFilterState
(filterParams)); } } - std::error_code resetParams(const OneEuroFilterParams &filterParams) - { + std::error_code resetParams(const OneEuroFilterParams& filterParams) { std::error_code err = ErrorCode::SUCCESS; - for (size_t i = 0; i < m_states.size(); i++) - { + for (size_t i = 0; i < m_states.size(); i++) { err = m_states[i]->resetParams(filterParams); - if (err != make_error_code(ErrorCode::SUCCESS)) - { + if (err != make_error_code(ErrorCode::SUCCESS)) { return err; } } @@ -215,30 +176,24 @@ struct OneEuroFilter::OneEuroFilterImpl ~OneEuroFilterImpl() {} - template::value == 1>::type * = nullptr> - std::error_code filter(U &outValue, U value) - { - if (m_states.size() != 1) - { + template::value == 1>::type * = nullptr> + std::error_code filter(U& outValue, U value) { + if (m_states.size() != 1) { return ErrorCode::INVALID_OPERATION; } std::error_code err = m_states[0]->filter(outValue, value); return err; } - template::value != 1>::type * = nullptr> - std::error_code filter(U &outValue, U value) - { - if (m_states.size() <= 1) - { + template::value != 1>::type * = nullptr> + std::error_code filter(U& outValue, U value) { + if (m_states.size() <= 1) { return ErrorCode::INVALID_OPERATION; } std::error_code err = ErrorCode::SUCCESS; - for (size_t i = 0; i < m_states.size(); i++) - { + for (size_t i = 0; i < m_states.size(); i++) { err = m_states[i]->filter(outValue[i], value[i]); - if (err != make_error_code(ErrorCode::SUCCESS)) - { + if (err != make_error_code(ErrorCode::SUCCESS)) { return err; } } @@ -250,31 +205,22 @@ struct OneEuroFilter::OneEuroFilterImpl }; template -OneEuroFilter::OneEuroFilter(const OneEuroFilterParams &filterParams) - : m_pImpl(new OneEuroFilterImpl(filterParams)) -{ +OneEuroFilter::OneEuroFilter(const OneEuroFilterParams& filterParams) + : m_pImpl(new OneEuroFilterImpl(filterParams)) { } template -OneEuroFilter::~OneEuroFilter() -{ +OneEuroFilter::~OneEuroFilter() { } template -std::error_code OneEuroFilter::resetParams(const OneEuroFilterParams &filterParams) -{ +std::error_code OneEuroFilter::resetParams(const OneEuroFilterParams& filterParams) { auto err = m_pImpl->resetParams(filterParams); return err; } template -std::error_code OneEuroFilter::execute(T &filteredValue, T inValue) -{ -#ifdef NVBENCH_ENABLE - std::string funcName = "OneEuroFilter_"; - std::string tag = funcName + typeid(T).name(); - nv::bench::Timer timerFunc = nv::bench::CPU(tag.c_str(), nv::bench::Flag::DEFAULT); -#endif +std::error_code OneEuroFilter::execute(T& filteredValue, T inValue) { auto err = m_pImpl->filter(filteredValue, inValue); return err; } @@ -285,4 +231,6 @@ template class OneEuroFilter; template class OneEuroFilter; template class OneEuroFilter; template class OneEuroFilter; -}} // namespace cvcore::tensor_ops + +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/OneEuroFilter.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/OneEuroFilter.h similarity index 67% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/OneEuroFilter.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/OneEuroFilter.h index 4947042..47d42cb 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/OneEuroFilter.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/OneEuroFilter.h @@ -15,68 +15,65 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_ONEEUROFILTER_H -#define CVCORE_ONEEUROFILTER_H +#pragma once #include #include -#include "cv/core/CVError.h" +#include "extensions/tensorops/core/CVError.h" -namespace cvcore { namespace tensor_ops { +namespace cvcore { +namespace tensor_ops { /** * Euro Filter Parameters */ -struct OneEuroFilterParams -{ +struct OneEuroFilterParams { float dataUpdateRate; /**< Data Update rate in Hz. */ float minCutoffFreq; /**< Minimum Cut off frequency in Hz. */ float cutoffSlope; /**< Beta or Speed coefficient which is a tuning parameter. */ float derivCutoffFreq; /**< Cutoff frequency for derivative. */ }; -/* +/* The one euro filter is a low pass filter for filtering noisy signals in real-time. The filtering uses exponential smoothing where the smooting factor is computed dynamically using the input data update rate. The smoothing factor provides a trade off between slow speed jitter vs high speed lag. There are two main tuning parameters for the filter, the speed coeffcient Beta and the minimum cut off frequency. If high speed lag is a problem, increase beta; if slow speed jitter is a problem, decrease fcmin. Reference : http://cristal.univ-lille.fr/~casiez/1euro/ */ template -class OneEuroFilter -{ -public: +class OneEuroFilter { + public: struct OneEuroFilterImpl; /** - * Euro Filter Constructor. - * @param filterParams Filter parameters - */ - OneEuroFilter(const OneEuroFilterParams &filterParams); + * Euro Filter Constructor. + * @param filterParams Filter parameters + */ + OneEuroFilter(const OneEuroFilterParams & filterParams); /** - * Reset Euro filter Parameters. - * @param filterParams Filter parameters - * @return Error code - */ - std::error_code resetParams(const OneEuroFilterParams &filterParams); + * Reset Euro filter Parameters. + * @param filterParams Filter parameters + * @return Error code + */ + std::error_code resetParams(const OneEuroFilterParams & filterParams); /** - * Filter the input. Supports float, double, vector2f, Vector3f, Vector3d, Vector3f input types. - * @param inValue Noisy input to be filtered. - * @param filteredValue Filtered output - * @return Error code - */ - std::error_code execute(T &filteredValue, T inValue); + * Filter the input. Supports float, double, vector2f, Vector3f, Vector3d, Vector3f input types. + * @param inValue Noisy input to be filtered. + * @param filteredValue Filtered output + * @return Error code + */ + std::error_code execute(T & filteredValue, T inValue); ~OneEuroFilter(); -private: + private: /** - * Implementation of EuroFilter. - */ + * Implementation of EuroFilter. + */ std::unique_ptr m_pImpl; }; -}} // namespace cvcore::tensor_ops - -#endif // CVCORE_ONEEUROFILTER_H +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/core/cvcore/Tensor.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Tensor.cpp similarity index 69% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/src/core/cvcore/Tensor.cpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Tensor.cpp index 83ede60..4752eac 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/core/cvcore/Tensor.cpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Tensor.cpp @@ -15,34 +15,34 @@ // // SPDX-License-Identifier: Apache-2.0 -#include "cv/core/Tensor.h" +#include "extensions/tensorops/core/Tensor.h" #include +#include #include #include -#include #include #include +#include namespace cvcore { +namespace tensor_ops { TensorBase::TensorBase() : m_data(nullptr) , m_dimCount(0) , m_type(U8) , m_isOwning(false) - , m_isCPU(true) -{ - for (int i = 0; i < kMaxDimCount; ++i) - { + , m_isCPU(true) { + for (int i = 0; i < kMaxDimCount; ++i) { m_dimData[i] = {0, 0}; } } -TensorBase::TensorBase(ChannelType type, const DimData *dimData, int dimCount, void *dataPtr, bool isCPU) - : TensorBase() -{ +TensorBase::TensorBase(ChannelType type, const DimData * dimData, + int dimCount, void * dataPtr, bool isCPU) + : TensorBase() { assert(dimCount >= kMinDimCount && dimCount <= kMaxDimCount); m_isOwning = false; @@ -50,69 +50,56 @@ TensorBase::TensorBase(ChannelType type, const DimData *dimData, int dimCount, v m_type = type; m_dimCount = dimCount; - for (int i = 0; i < dimCount; ++i) - { + for (int i = 0; i < dimCount; ++i) { m_dimData[i] = dimData[i]; } m_data = dataPtr; } -TensorBase::TensorBase(ChannelType type, std::initializer_list dimData, void *dataPtr, bool isCPU) - : TensorBase(type, dimData.begin(), dimData.size(), dataPtr, isCPU) -{ +TensorBase::TensorBase(ChannelType type, std::initializer_list dimData, + void * dataPtr, bool isCPU) + : TensorBase(type, dimData.begin(), dimData.size(), dataPtr, isCPU) { } -TensorBase::TensorBase(ChannelType type, const DimData *dimData, int dimCount, bool isCPU) - : TensorBase(type, dimData, dimCount, nullptr, isCPU) -{ +TensorBase::TensorBase(ChannelType type, const DimData * dimData, int dimCount, bool isCPU) + : TensorBase(type, dimData, dimCount, nullptr, isCPU) { m_isOwning = true; // compute tensor memory block size const std::size_t tensorSize = getDataSize(); // allocate - if (isCPU) - { + if (isCPU) { m_data = std::malloc(tensorSize); - } - else - { - if (cudaMalloc(&m_data, tensorSize) != 0) - { + } else { + if (cudaMalloc(&m_data, tensorSize) != 0) { throw std::runtime_error("CUDA alloc() failed!"); } } } -TensorBase::TensorBase(ChannelType type, std::initializer_list dimData, bool isCPU) - : TensorBase(type, dimData.begin(), dimData.size(), isCPU) -{ +TensorBase::TensorBase(ChannelType type, std::initializer_list dimData, + bool isCPU) + : TensorBase(type, dimData.begin(), dimData.size(), isCPU) { } -TensorBase::~TensorBase() -{ - if (m_isOwning) - { - if (m_isCPU) - { +TensorBase::~TensorBase() { + if (m_isOwning) { + if (m_isCPU) { std::free(m_data); - } - else - { + } else { cudaFree(m_data); } } } -TensorBase::TensorBase(TensorBase &&t) - : TensorBase() -{ +TensorBase::TensorBase(TensorBase&& t) + : TensorBase() { *this = std::move(t); } -TensorBase &TensorBase::operator=(TensorBase &&t) -{ +TensorBase& TensorBase::operator=(TensorBase&& t) { using std::swap; swap(m_data, t.m_data); @@ -121,66 +108,54 @@ TensorBase &TensorBase::operator=(TensorBase &&t) swap(m_isOwning, t.m_isOwning); swap(m_isCPU, t.m_isCPU); - for (int i = 0; i < kMaxDimCount; ++i) - { + for (int i = 0; i < kMaxDimCount; ++i) { swap(m_dimData[i], t.m_dimData[i]); } return *this; } -int TensorBase::getDimCount() const -{ +int TensorBase::getDimCount() const { return m_dimCount; } -std::size_t TensorBase::getSize(int dimIdx) const -{ +std::size_t TensorBase::getSize(int dimIdx) const { assert(dimIdx >= 0 && dimIdx < m_dimCount); return m_dimData[dimIdx].size; } -std::size_t TensorBase::getStride(int dimIdx) const -{ +std::size_t TensorBase::getStride(int dimIdx) const { assert(dimIdx >= 0 && dimIdx < m_dimCount); return m_dimData[dimIdx].stride; } -ChannelType TensorBase::getType() const -{ +ChannelType TensorBase::getType() const { return m_type; } -void *TensorBase::getData() const -{ +void* TensorBase::getData() const { return m_data; } -std::size_t TensorBase::getDataSize() const -{ +std::size_t TensorBase::getDataSize() const { std::size_t tensorSize = m_dimData[0].size * m_dimData[0].stride; - for (int i = 1; i < m_dimCount; ++i) - { + for (int i = 1; i < m_dimCount; ++i) { tensorSize = std::max(tensorSize, m_dimData[i].size * m_dimData[i].stride); } tensorSize *= GetChannelSize(m_type); return tensorSize; } -bool TensorBase::isCPU() const -{ +bool TensorBase::isCPU() const { return m_isCPU; } -bool TensorBase::isOwning() const -{ +bool TensorBase::isOwning() const { return m_isOwning; } -std::string GetTensorLayoutAsString(TensorLayout TL) -{ - switch (TL) - { +std::string GetTensorLayoutAsString(TensorLayout TL) { + switch (TL) { case TensorLayout::CL: return "CL"; case TensorLayout::LC: @@ -200,10 +175,8 @@ std::string GetTensorLayoutAsString(TensorLayout TL) } } -std::string GetChannelCountAsString(ChannelCount CC) -{ - switch (CC) - { +std::string GetChannelCountAsString(ChannelCount CC) { + switch (CC) { case ChannelCount::C1: return "C1"; case ChannelCount::C2: @@ -219,10 +192,8 @@ std::string GetChannelCountAsString(ChannelCount CC) } } -std::string GetChannelTypeAsString(ChannelType CT) -{ - switch (CT) - { +std::string GetChannelTypeAsString(ChannelType CT) { + switch (CT) { case ChannelType::U8: return "U8"; case ChannelType::U16: @@ -242,10 +213,8 @@ std::string GetChannelTypeAsString(ChannelType CT) } } -std::size_t GetChannelSize(ChannelType CT) -{ - switch (CT) - { +std::size_t GetChannelSize(ChannelType CT) { + switch (CT) { case U8: case S8: return 1; @@ -262,9 +231,9 @@ std::size_t GetChannelSize(ChannelType CT) } } -std::string GetMemoryTypeAsString(bool isCPU) -{ +std::string GetMemoryTypeAsString(bool isCPU) { return isCPU? "CPU" : "GPU"; } -} // namespace cvcore +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/Tensor.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Tensor.h similarity index 65% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/Tensor.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Tensor.h index b06e531..4323dc5 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/Tensor.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Tensor.h @@ -15,8 +15,7 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_TENSOR_H -#define CVCORE_TENSOR_H +#pragma once #include #include @@ -24,6 +23,7 @@ #include namespace cvcore { +namespace tensor_ops { // there is no CUDA dependency at the data level, so we map half type to uint16_t for now using half = std::uint16_t; @@ -32,8 +32,7 @@ using half = std::uint16_t; * An enum. * Enum type for tensor layout type. */ -enum TensorLayout -{ +enum TensorLayout { LC, /**< length, channel (channel interleaved). */ CL, /**< channel, length (channel planar). */ HWC, /**< height, width, channel (channel interleaved). */ @@ -49,8 +48,7 @@ enum TensorLayout * An enum. * Enum type for tensor channel count. */ -enum ChannelCount -{ +enum ChannelCount { C1, /**< 1 channels. */ C2, /**< 2 channels. */ C3, /**< 3 channels. */ @@ -62,13 +60,12 @@ enum ChannelCount * An enum. * Enum type for channel type. */ -enum ChannelType -{ +enum ChannelType { U8, /**< uint8_t. */ U16, /**< uint16_t. */ S8, /**< int8_t. */ S16, /**< int16_t. */ - F16, /**< cvcore::half. */ + F16, /**< cvcore::tensor_ops::half. */ F32, /**< float. */ F64, /**< double. */ }; @@ -77,8 +74,7 @@ enum ChannelType * An enum. * Enum type for dimension type. */ -enum class TensorDimension -{ +enum class TensorDimension { LENGTH, /**< length dimension. */ HEIGHT, /**< height dimension. */ WIDTH, /**< width dimension. */ @@ -124,14 +120,12 @@ std::size_t GetChannelSize(ChannelType CT); /** * Implementation of TensorBase class. */ -class TensorBase -{ -public: +class TensorBase { + public: /** * Struct for storing dimension data. */ - struct DimData - { + struct DimData { std::size_t size; /**< size of each dimension. */ std::size_t stride; /**< stride of each dimension. */ }; @@ -144,7 +138,8 @@ class TensorBase * @param dataPtr raw pointer to the source data array. * @param isCPU whether to allocate tensor on CPU or GPU. */ - TensorBase(ChannelType type, const DimData *dimData, int dimCount, void *dataPtr, bool isCPU); + TensorBase(ChannelType type, const DimData * dimData, int dimCount, + void * dataPtr, bool isCPU); /** * Constructor of a non-owning tensor. @@ -153,7 +148,8 @@ class TensorBase * @param dataPtr raw pointer to the source data array. * @param isCPU whether to allocate tensor on CPU or GPU. */ - TensorBase(ChannelType type, std::initializer_list dimData, void *dataPtr, bool isCPU); + TensorBase(ChannelType type, std::initializer_list dimData, + void * dataPtr, bool isCPU); /** * Constructor of a memory-owning tensor. @@ -162,7 +158,7 @@ class TensorBase * @param dimCount number of dimensions. * @param isCPU whether to allocate tensor on CPU or GPU. */ - TensorBase(ChannelType type, const DimData *dimData, int dimCount, bool isCPU); + TensorBase(ChannelType type, const DimData * dimData, int dimCount, bool isCPU); /** * Constructor of a memory-owning tensor. @@ -180,12 +176,12 @@ class TensorBase /** * TensorBase is non-copyable. */ - TensorBase(const TensorBase &) = delete; + TensorBase(const TensorBase&) = delete; /** * TensorBase is non-copyable. */ - TensorBase &operator=(const TensorBase &) = delete; + TensorBase& operator=(const TensorBase&) = delete; /** * Move Constructor of TensorBase. @@ -195,7 +191,7 @@ class TensorBase /** * Move operator of TensorBase. */ - TensorBase &operator=(TensorBase &&); + TensorBase& operator=(TensorBase&&); /** * Get the dimension count of TensorBase. @@ -227,7 +223,7 @@ class TensorBase * Get the raw data pointer to the Tensor. * @return void data pointer to the Tensor. */ - void *getData() const; + void* getData() const; /** * Get the total size of the Tensor in bytes. @@ -247,14 +243,14 @@ class TensorBase */ bool isOwning() const; -protected: + protected: TensorBase(); -private: + private: static constexpr int kMinDimCount = 2; static constexpr int kMaxDimCount = 4; - void *m_data; + void* m_data; int m_dimCount; DimData m_dimData[kMaxDimCount]; @@ -267,16 +263,14 @@ class TensorBase namespace detail { template -struct DimToIndex2D -{ +struct DimToIndex2D { static_assert(TL == LC || TL == CL, "unsupported variant!"); static constexpr int kLength = TL == LC ? 0 : 1; static constexpr int kChannel = TL == LC ? 1 : 0; }; template -struct DimToIndex3D -{ +struct DimToIndex3D { static_assert(TL == HWC || TL == CHW, "unsupported variant!"); static constexpr int kWidth = TL == HWC ? 1 : 2; static constexpr int kHeight = TL == HWC ? 0 : 1; @@ -284,8 +278,7 @@ struct DimToIndex3D }; template -struct DimToIndex4D -{ +struct DimToIndex4D { static_assert(TL == DHWC || TL == DCHW || TL == CDHW, "unsupported variant!"); static constexpr int kWidth = TL == DHWC ? 2 : (TL == DCHW ? 3 : 3); static constexpr int kHeight = TL == DHWC ? 1 : (TL == DCHW ? 2 : 2); @@ -294,91 +287,78 @@ struct DimToIndex4D }; template -struct LayoutToIndex -{ -}; +struct LayoutToIndex {}; template -struct LayoutToIndex::type> : public DimToIndex2D -{ +struct LayoutToIndex::type> : public DimToIndex2D { static constexpr int kDimCount = 2; }; template -struct LayoutToIndex::type> : public DimToIndex3D -{ +struct LayoutToIndex::type> : public DimToIndex3D { static constexpr int kDimCount = 3; }; template -struct LayoutToIndex::type> - : public DimToIndex4D -{ +struct LayoutToIndex::type> + : public DimToIndex4D { static constexpr int kDimCount = 4; }; template -struct ChannelTypeToNative -{ -}; +struct ChannelTypeToNative {}; template<> -struct ChannelTypeToNative -{ +struct ChannelTypeToNative { using Type = std::uint8_t; }; template<> -struct ChannelTypeToNative -{ +struct ChannelTypeToNative { using Type = std::uint16_t; }; template<> -struct ChannelTypeToNative -{ +struct ChannelTypeToNative { using Type = std::int8_t; }; template<> -struct ChannelTypeToNative -{ +struct ChannelTypeToNative { using Type = std::int16_t; }; template<> -struct ChannelTypeToNative -{ +struct ChannelTypeToNative { using Type = float; }; template<> -struct ChannelTypeToNative -{ - using Type = cvcore::half; +struct ChannelTypeToNative { + using Type = cvcore::tensor_ops::half; }; template<> -struct ChannelTypeToNative -{ +struct ChannelTypeToNative { using Type = double; }; template -constexpr std::size_t ChannelToCount() -{ - switch (CC) - { - case C1: - return 1; - case C2: - return 2; - case C3: - return 3; - case C4: - return 4; - } - return 0; // this is safe as this function will never be called for dynamic channel counts +constexpr std::size_t ChannelToCount() { + switch (CC) { + case C1: + return 1; + case C2: + return 2; + case C3: + return 3; + case C4: + return 4; + } + return 0; // this is safe as this function will never be called for dynamic channel counts } /** @@ -387,11 +367,10 @@ constexpr std::size_t ChannelToCount() * @tparam CT channel type. */ template -class Tensor2D : public TensorBase -{ +class Tensor2D : public TensorBase { using DataType = typename ChannelTypeToNative::Type; -public: + public: /** * Default Constructor. */ @@ -403,8 +382,7 @@ class Tensor2D : public TensorBase * @param isCPU whether to allocate tensor on CPU or GPU. */ Tensor2D(std::initializer_list dimData, bool isCPU) - : TensorBase(CT, dimData, isCPU) - { + : TensorBase(CT, dimData, isCPU) { } /** @@ -413,17 +391,15 @@ class Tensor2D : public TensorBase * @param dataPtr raw pointer to the source data array. * @param isCPU whether to allocate tensor on CPU or GPU. */ - Tensor2D(std::initializer_list dimData, DataType *dataPtr, bool isCPU) - : TensorBase(CT, dimData, dataPtr, isCPU) - { + Tensor2D(std::initializer_list dimData, DataType * dataPtr, bool isCPU) + : TensorBase(CT, dimData, dataPtr, isCPU) { } /** * Get the length of the 2D tensor. * @return length of the 2D tensor. */ - std::size_t getLength() const - { + std::size_t getLength() const { return getSize(DimToIndex2D::kLength); } @@ -431,8 +407,7 @@ class Tensor2D : public TensorBase * Get the channel count of the 2D tensor. * @return channel count of the 2D tensor. */ - std::size_t getChannelCount() const - { + std::size_t getChannelCount() const { return getSize(DimToIndex2D::kChannel); } @@ -446,16 +421,15 @@ class Tensor2D : public TensorBase * @param dim tensor dimension. * @return tensor stride of the given dimension. */ - std::size_t getStride(TensorDimension dim) const - { - switch (dim) - { - case TensorDimension::LENGTH: - return getStride(DimToIndex2D::kLength); - case TensorDimension::CHANNEL: - return getStride(DimToIndex2D::kChannel); - default: - throw std::out_of_range("cvcore::Tensor2D::getStride ==> Requested TensorDimension is out of bounds"); + std::size_t getStride(TensorDimension dim) const { + switch (dim) { + case TensorDimension::LENGTH: + return getStride(DimToIndex2D::kLength); + case TensorDimension::CHANNEL: + return getStride(DimToIndex2D::kChannel); + default: + throw std::out_of_range( + "cvcore::Tensor2D::getStride ==> Requested TensorDimension is out of bound"); } } @@ -463,8 +437,7 @@ class Tensor2D : public TensorBase * Get the raw data pointer to the 2D tensor. * @return data pointer to the 2D tensor. */ - DataType *getData() - { + DataType* getData() { return reinterpret_cast(TensorBase::getData()); } @@ -472,8 +445,7 @@ class Tensor2D : public TensorBase * Get the const raw data pointer to the 2D tensor. * @return const data pointer to the 2D tensor. */ - const DataType *getData() const - { + const DataType* getData() const { return reinterpret_cast(TensorBase::getData()); } }; @@ -484,11 +456,10 @@ class Tensor2D : public TensorBase * @tparam CT channel type. */ template -class Tensor3D : public TensorBase -{ +class Tensor3D : public TensorBase { using DataType = typename ChannelTypeToNative::Type; -public: + public: /** * Default Constructor. */ @@ -500,8 +471,7 @@ class Tensor3D : public TensorBase * @param isCPU whether to allocate tensor on CPU or GPU. */ Tensor3D(std::initializer_list dimData, bool isCPU) - : TensorBase(CT, dimData, isCPU) - { + : TensorBase(CT, dimData, isCPU) { } /** @@ -510,17 +480,15 @@ class Tensor3D : public TensorBase * @param dataPtr raw pointer to the source data array. * @param isCPU whether to allocate tensor on CPU or GPU. */ - Tensor3D(std::initializer_list dimData, DataType *dataPtr, bool isCPU) - : TensorBase(CT, dimData, dataPtr, isCPU) - { + Tensor3D(std::initializer_list dimData, DataType * dataPtr, bool isCPU) + : TensorBase(CT, dimData, dataPtr, isCPU) { } /** * Get the width of the 3D tensor. * @return width of the 3D tensor. */ - std::size_t getWidth() const - { + std::size_t getWidth() const { return getSize(DimToIndex3D::kWidth); } @@ -528,8 +496,7 @@ class Tensor3D : public TensorBase * Get the height of the 3D tensor. * @return height of the 3D tensor. */ - std::size_t getHeight() const - { + std::size_t getHeight() const { return getSize(DimToIndex3D::kHeight); } @@ -537,8 +504,7 @@ class Tensor3D : public TensorBase * Get the channel count of the 3D tensor. * @return channel count of the 3D tensor. */ - std::size_t getChannelCount() const - { + std::size_t getChannelCount() const { return getSize(DimToIndex3D::kChannel); } @@ -552,18 +518,17 @@ class Tensor3D : public TensorBase * @param dim tensor dimension. * @return tensor stride of the given dimension. */ - std::size_t getStride(TensorDimension dim) const - { - switch (dim) - { - case TensorDimension::HEIGHT: - return getStride(DimToIndex3D::kHeight); - case TensorDimension::WIDTH: - return getStride(DimToIndex3D::kWidth); - case TensorDimension::CHANNEL: - return getStride(DimToIndex3D::kChannel); - default: - throw std::out_of_range("cvcore::Tensor3D::getStride ==> Requested TensorDimension is out of bounds"); + std::size_t getStride(TensorDimension dim) const { + switch (dim) { + case TensorDimension::HEIGHT: + return getStride(DimToIndex3D::kHeight); + case TensorDimension::WIDTH: + return getStride(DimToIndex3D::kWidth); + case TensorDimension::CHANNEL: + return getStride(DimToIndex3D::kChannel); + default: + throw std::out_of_range( + "cvcore::Tensor3D::getStride ==>Requested TensorDimension is out of bounds"); } } @@ -571,8 +536,7 @@ class Tensor3D : public TensorBase * Get the raw data pointer to the 3D tensor. * @return data pointer to the 3D tensor. */ - DataType *getData() - { + DataType* getData() { return reinterpret_cast(TensorBase::getData()); } @@ -580,8 +544,7 @@ class Tensor3D : public TensorBase * Get the const raw data pointer to the 3D tensor. * @return const data pointer to the 3D tensor. */ - const DataType *getData() const - { + const DataType* getData() const { return reinterpret_cast(TensorBase::getData()); } }; @@ -592,11 +555,10 @@ class Tensor3D : public TensorBase * @tparam CT channel type. */ template -class Tensor4D : public TensorBase -{ +class Tensor4D : public TensorBase { using DataType = typename ChannelTypeToNative::Type; -public: + public: /** * Default Constructor. */ @@ -608,8 +570,7 @@ class Tensor4D : public TensorBase * @param isCPU whether to allocate tensor on CPU or GPU. */ Tensor4D(std::initializer_list dimData, bool isCPU) - : TensorBase(CT, dimData, isCPU) - { + : TensorBase(CT, dimData, isCPU) { } /** @@ -618,17 +579,15 @@ class Tensor4D : public TensorBase * @param dataPtr raw pointer to the source data array. * @param isCPU whether to allocate tensor on CPU or GPU. */ - Tensor4D(std::initializer_list dimData, DataType *dataPtr, bool isCPU) - : TensorBase(CT, dimData, dataPtr, isCPU) - { + Tensor4D(std::initializer_list dimData, DataType * dataPtr, bool isCPU) + : TensorBase(CT, dimData, dataPtr, isCPU) { } /** * Get the width of the 4D tensor. * @return width of the 4D tensor. */ - std::size_t getWidth() const - { + std::size_t getWidth() const { return getSize(DimToIndex4D::kWidth); } @@ -636,8 +595,7 @@ class Tensor4D : public TensorBase * Get the height of the 4D tensor. * @return height of the 4D tensor. */ - std::size_t getHeight() const - { + std::size_t getHeight() const { return getSize(DimToIndex4D::kHeight); } @@ -645,8 +603,7 @@ class Tensor4D : public TensorBase * Get the depth of the 4D tensor. * @return depth of the 4D tensor. */ - std::size_t getDepth() const - { + std::size_t getDepth() const { return getSize(DimToIndex4D::kDepth); } @@ -654,8 +611,7 @@ class Tensor4D : public TensorBase * Get the channel count of the 4D tensor. * @return channel count of the 4D tensor. */ - std::size_t getChannelCount() const - { + std::size_t getChannelCount() const { return getSize(DimToIndex4D::kChannel); } @@ -669,20 +625,19 @@ class Tensor4D : public TensorBase * @param dim tensor dimension. * @return tensor stride of the given dimension. */ - std::size_t getStride(TensorDimension dim) const - { - switch (dim) - { - case TensorDimension::HEIGHT: - return getStride(DimToIndex4D::kHeight); - case TensorDimension::WIDTH: - return getStride(DimToIndex4D::kWidth); - case TensorDimension::CHANNEL: - return getStride(DimToIndex4D::kChannel); - case TensorDimension::DEPTH: - return getStride(DimToIndex4D::kDepth); - default: - throw std::out_of_range("cvcore::Tensor4D::getStride ==> Requested TensorDimension is out of bounds"); + std::size_t getStride(TensorDimension dim) const { + switch (dim) { + case TensorDimension::HEIGHT: + return getStride(DimToIndex4D::kHeight); + case TensorDimension::WIDTH: + return getStride(DimToIndex4D::kWidth); + case TensorDimension::CHANNEL: + return getStride(DimToIndex4D::kChannel); + case TensorDimension::DEPTH: + return getStride(DimToIndex4D::kDepth); + default: + throw std::out_of_range( + "cvcore::Tensor4D::getStride ==>Requested TensorDimension is out of bounds"); } } @@ -690,8 +645,7 @@ class Tensor4D : public TensorBase * Get the raw data pointer to the 4D tensor. * @return data pointer to the 4D tensor. */ - DataType *getData() - { + DataType* getData() { return reinterpret_cast(TensorBase::getData()); } @@ -699,13 +653,12 @@ class Tensor4D : public TensorBase * Get the const raw data pointer to the 4D tensor. * @return const data pointer to the 4D tensor. */ - const DataType *getData() const - { + const DataType* getData() const { return reinterpret_cast(TensorBase::getData()); } }; -} // namespace detail +} // namespace detail template class Tensor; @@ -718,9 +671,8 @@ class Tensor; * @tparam CT channel type. */ template -class Tensor : public detail::Tensor2D -{ -public: +class Tensor : public detail::Tensor2D { + public: using DataType = typename detail::ChannelTypeToNative::Type; static constexpr ChannelCount kChannelCount = CC; @@ -729,27 +681,24 @@ class Tensor : public detail::Tensor2D template::type> Tensor(std::size_t length, bool isCPU = true) - : detail::Tensor2D({{length, detail::ChannelToCount()}, {detail::ChannelToCount(), 1}}, isCPU) - { + : detail::Tensor2D({{length, detail::ChannelToCount()}, + {detail::ChannelToCount(), 1}}, isCPU) { } template::type> Tensor(std::size_t length, std::size_t channelCount, bool isCPU = true) - : detail::Tensor2D({{length, channelCount}, {channelCount, 1}}, isCPU) - { + : detail::Tensor2D({{length, channelCount}, {channelCount, 1}}, isCPU) { } template::type> - Tensor(std::size_t length, DataType *dataPtr, bool isCPU = true) - : detail::Tensor2D({{length, detail::ChannelToCount()}, {detail::ChannelToCount(), 1}}, dataPtr, - isCPU) - { + Tensor(std::size_t length, DataType * dataPtr, bool isCPU = true) + : detail::Tensor2D({{length, detail::ChannelToCount()}, + {detail::ChannelToCount(), 1}}, dataPtr, isCPU) { } template::type> - Tensor(std::size_t length, std::size_t channelCount, DataType *dataPtr, bool isCPU = true) - : detail::Tensor2D({{length, channelCount}, {channelCount, 1}}, dataPtr, isCPU) - { + Tensor(std::size_t length, std::size_t channelCount, DataType * dataPtr, bool isCPU = true) + : detail::Tensor2D({{length, channelCount}, {channelCount, 1}}, dataPtr, isCPU) { } }; @@ -759,9 +708,8 @@ class Tensor : public detail::Tensor2D * @tparam CT channel type. */ template -class Tensor : public detail::Tensor2D -{ -public: +class Tensor : public detail::Tensor2D { + public: using DataType = typename detail::ChannelTypeToNative::Type; static constexpr ChannelCount kChannelCount = CC; @@ -770,26 +718,24 @@ class Tensor : public detail::Tensor2D template::type> Tensor(std::size_t length, bool isCPU = true) - : detail::Tensor2D({{detail::ChannelToCount(), length}, {length, 1}}, isCPU) - { + : detail::Tensor2D({{detail::ChannelToCount(), length}, + {length, 1}}, isCPU) { } template::type> Tensor(std::size_t length, std::size_t channelCount, bool isCPU = true) - : detail::Tensor2D({{channelCount, length}, {length, 1}}, isCPU) - { + : detail::Tensor2D({{channelCount, length}, {length, 1}}, isCPU) { } template::type> - Tensor(std::size_t length, DataType *dataPtr, bool isCPU = true) - : detail::Tensor2D({{detail::ChannelToCount(), length}, {length, 1}}, dataPtr, isCPU) - { + Tensor(std::size_t length, DataType * dataPtr, bool isCPU = true) + : detail::Tensor2D({{detail::ChannelToCount(), length}, + {length, 1}}, dataPtr, isCPU) { } template::type> - Tensor(std::size_t length, std::size_t channelCount, DataType *dataPtr, bool isCPU = true) - : detail::Tensor2D({{channelCount, length}, {length, 1}}, dataPtr, isCPU) - { + Tensor(std::size_t length, std::size_t channelCount, DataType * dataPtr, bool isCPU = true) + : detail::Tensor2D({{channelCount, length}, {length, 1}}, dataPtr, isCPU) { } }; @@ -801,9 +747,8 @@ class Tensor : public detail::Tensor2D * @tparam CT channel type. */ template -class Tensor : public detail::Tensor3D -{ -public: +class Tensor : public detail::Tensor3D { + public: using DataType = typename detail::ChannelTypeToNative::Type; static constexpr ChannelCount kChannelCount = CC; @@ -816,61 +761,56 @@ class Tensor : public detail::Tensor3D : detail::Tensor3D({{height, width * detail::ChannelToCount()}, {width, detail::ChannelToCount()}, {detail::ChannelToCount(), 1}}, - isCPU) - { + isCPU) { } template::value>::type * = nullptr> Tensor(std::size_t width, std::size_t height, std::size_t channelCount, B isCPU = true) - : detail::Tensor3D({{height, width * channelCount}, {width, channelCount}, {channelCount, 1}}, isCPU) - { + : detail::Tensor3D({{height, width * channelCount}, {width, channelCount}, + {channelCount, 1}}, isCPU) { } template::value>::type * = nullptr> - Tensor(std::size_t width, std::size_t height, DataType *dataPtr, B isCPU = true) + Tensor(std::size_t width, std::size_t height, DataType * dataPtr, B isCPU = true) : detail::Tensor3D({{height, width * detail::ChannelToCount()}, {width, detail::ChannelToCount()}, {detail::ChannelToCount(), 1}}, - dataPtr, isCPU) - { + dataPtr, isCPU) { } template::value>::type * = nullptr> - Tensor(std::size_t width, std::size_t height, std::size_t rowPitch, DataType *dataPtr, B isCPU = true) + Tensor(std::size_t width, std::size_t height, std::size_t rowPitch, + DataType * dataPtr, B isCPU = true) : detail::Tensor3D({{height, rowPitch / GetChannelSize(CT)}, {width, detail::ChannelToCount()}, {detail::ChannelToCount(), 1}}, - dataPtr, isCPU) - { - if (rowPitch % GetChannelSize(CT) != 0) - { + dataPtr, isCPU) { + if (rowPitch % GetChannelSize(CT) != 0) { throw std::domain_error( - "cvcore::Tensor::Tensor ==> Parameter rowPitch is not evenly divisible by channel size"); + "cvcore::Tensor::Tensor ==> Parameter rowPitch error"); } } template::value>::type * = nullptr> - Tensor(std::size_t width, std::size_t height, std::size_t channelCount, DataType *dataPtr, B isCPU = true) - : detail::Tensor3D({{height, width * channelCount}, {width, channelCount}, {channelCount, 1}}, dataPtr, - isCPU) - { + Tensor(std::size_t width, std::size_t height, std::size_t channelCount, + DataType * dataPtr, B isCPU = true) + : detail::Tensor3D({{height, width * channelCount}, + {width, channelCount}, {channelCount, 1}}, dataPtr, isCPU) { } template::value>::type * = nullptr> - Tensor(std::size_t width, std::size_t height, std::size_t channelCount, std::size_t rowPitch, DataType *dataPtr, - B isCPU = true) - : detail::Tensor3D({{height, rowPitch / GetChannelSize(CT)}, {width, channelCount}, {channelCount, 1}}, - dataPtr, isCPU) - { - if (rowPitch % GetChannelSize(CT) != 0) - { + Tensor(std::size_t width, std::size_t height, std::size_t channelCount, + std::size_t rowPitch, DataType * dataPtr, B isCPU = true) + : detail::Tensor3D({{height, rowPitch / GetChannelSize(CT)}, + {width, channelCount}, {channelCount, 1}}, dataPtr, isCPU) { + if (rowPitch % GetChannelSize(CT) != 0) { throw std::domain_error( - "cvcore::Tensor::Tensor ==> Parameter rowPitch is not evenly divisible by channel size"); + "cvcore::Tensor::Tensor ==> Parameter rowPitch error"); } } }; @@ -881,9 +821,8 @@ class Tensor : public detail::Tensor3D * @tparam CT channel type. */ template -class Tensor : public detail::Tensor3D -{ -public: +class Tensor : public detail::Tensor3D { + public: using DataType = typename detail::ChannelTypeToNative::Type; static constexpr ChannelCount kChannelCount = CC; @@ -892,58 +831,53 @@ class Tensor : public detail::Tensor3D template::type> Tensor(std::size_t width, std::size_t height, bool isCPU = true) - : detail::Tensor3D({{detail::ChannelToCount(), width * height}, {height, width}, {width, 1}}, - isCPU) - { + : detail::Tensor3D({{detail::ChannelToCount(), width * height}, + {height, width}, {width, 1}}, isCPU) { } template::type> Tensor(std::size_t width, std::size_t height, std::size_t channelCount, bool isCPU = true) - : detail::Tensor3D({{channelCount, width * height}, {height, width}, {width, 1}}, isCPU) - { + : detail::Tensor3D({{channelCount, width * height}, + {height, width}, {width, 1}}, isCPU) { } template::type> - Tensor(std::size_t width, std::size_t height, DataType *dataPtr, bool isCPU = true) - : detail::Tensor3D({{detail::ChannelToCount(), width * height}, {height, width}, {width, 1}}, - dataPtr, isCPU) - { + Tensor(std::size_t width, std::size_t height, DataType * dataPtr, bool isCPU = true) + : detail::Tensor3D({{detail::ChannelToCount(), width * height}, + {height, width}, {width, 1}}, dataPtr, isCPU) { } template::value>::type * = nullptr> - Tensor(std::size_t width, std::size_t height, std::size_t rowPitch, DataType *dataPtr, B isCPU = true) - : detail::Tensor3D({{detail::ChannelToCount(), height * rowPitch / GetChannelSize(CT)}, - {height, rowPitch / GetChannelSize(CT)}, - {width, 1}}, - dataPtr, isCPU) - { - if (rowPitch % GetChannelSize(CT) != 0) - { + Tensor(std::size_t width, std::size_t height, std::size_t rowPitch, + DataType * dataPtr, B isCPU = true) + : detail::Tensor3D({{detail::ChannelToCount(), + height * rowPitch / GetChannelSize(CT)}, + {height, rowPitch / GetChannelSize(CT)}, + {width, 1}}, dataPtr, isCPU) { + if (rowPitch % GetChannelSize(CT) != 0) { throw std::domain_error( - "cvcore::Tensor::Tensor ==> Parameter rowPitch is not evenly divisible by channel size"); + "cvcore::Tensor::Tensor ==> Parameter rowPitch error"); } } template::type> - Tensor(std::size_t width, std::size_t height, std::size_t channelCount, DataType *dataPtr, bool isCPU = true) - : detail::Tensor3D({{channelCount, width * height}, {height, width}, {width, 1}}, dataPtr, isCPU) - { + Tensor(std::size_t width, std::size_t height, std::size_t channelCount, + DataType * dataPtr, bool isCPU = true) + : detail::Tensor3D({{channelCount, width * height}, + {height, width}, {width, 1}}, dataPtr, isCPU) { } template::value>::type * = nullptr> - Tensor(std::size_t width, std::size_t height, std::size_t channelCount, std::size_t rowPitch, DataType *dataPtr, - B isCPU = true) + Tensor(std::size_t width, std::size_t height, std::size_t channelCount, + std::size_t rowPitch, DataType * dataPtr, B isCPU = true) : detail::Tensor3D({{channelCount, height * rowPitch / GetChannelSize(CT)}, - {height, rowPitch / GetChannelSize(CT)}, - {width, 1}}, - dataPtr, isCPU) - { - if (rowPitch % GetChannelSize(CT) != 0) - { + {height, rowPitch / GetChannelSize(CT)}, + {width, 1}}, dataPtr, isCPU) { + if (rowPitch % GetChannelSize(CT) != 0) { throw std::domain_error( - "cvcore::Tensor::Tensor ==> Parameter rowPitch is not evenly divisible by channel size"); + "cvcore::Tensor::Tensor ==> Parameter rowPitch error"); } } }; @@ -956,9 +890,8 @@ class Tensor : public detail::Tensor3D * @tparam CT channel type. */ template -class Tensor : public detail::Tensor4D -{ -public: +class Tensor : public detail::Tensor4D { + public: using DataType = typename detail::ChannelTypeToNative::Type; static constexpr ChannelCount kChannelCount = CC; @@ -969,78 +902,63 @@ class Tensor : public detail::Tensor4D typename std::enable_if::value>::type * = nullptr> Tensor(std::size_t width, std::size_t height, std::size_t depth, B isCPU = true) : detail::Tensor4D({{depth, height * width * detail::ChannelToCount()}, - {height, width * detail::ChannelToCount()}, - {width, detail::ChannelToCount()}, - {detail::ChannelToCount(), 1}}, - isCPU) - { + {height, width * detail::ChannelToCount()}, + {width, detail::ChannelToCount()}, + {detail::ChannelToCount(), 1}}, isCPU) { } template::value>::type * = nullptr> - Tensor(std::size_t width, std::size_t height, std::size_t depth, std::size_t channelCount, B isCPU = true) + Tensor(std::size_t width, std::size_t height, std::size_t depth, + std::size_t channelCount, B isCPU = true) : detail::Tensor4D({{depth, height * width * channelCount}, - {height, width * channelCount}, - {width, channelCount}, - {channelCount, 1}}, - isCPU) - { + {height, width * channelCount}, + {width, channelCount}, {channelCount, 1}}, isCPU) { } template::value>::type * = nullptr> - Tensor(std::size_t width, std::size_t height, std::size_t depth, DataType *dataPtr, B isCPU = true) + Tensor(std::size_t width, std::size_t height, std::size_t depth, + DataType * dataPtr, B isCPU = true) : detail::Tensor4D({{depth, height * width * detail::ChannelToCount()}, - {height, width * detail::ChannelToCount()}, - {width, detail::ChannelToCount()}, - {detail::ChannelToCount(), 1}}, - dataPtr, isCPU) - { + {height, width * detail::ChannelToCount()}, + {width, detail::ChannelToCount()}, + {detail::ChannelToCount(), 1}}, dataPtr, isCPU) { } template::value>::type * = nullptr> - Tensor(std::size_t width, std::size_t height, std::size_t depth, std::size_t rowPitch, DataType *dataPtr, - B isCPU = true) + Tensor(std::size_t width, std::size_t height, std::size_t depth, + std::size_t rowPitch, DataType * dataPtr, B isCPU = true) : detail::Tensor4D({{depth, height * rowPitch / GetChannelSize(CT)}, - {height, rowPitch / GetChannelSize(CT)}, - {width, detail::ChannelToCount()}, - {detail::ChannelToCount(), 1}}, - dataPtr, isCPU) - { - if (rowPitch % GetChannelSize(CT) != 0) - { + {height, rowPitch / GetChannelSize(CT)}, + {width, detail::ChannelToCount()}, + {detail::ChannelToCount(), 1}}, dataPtr, isCPU) { + if (rowPitch % GetChannelSize(CT) != 0) { throw std::domain_error( - "cvcore::Tensor::Tensor ==> Parameter rowPitch is not evenly divisible by channel size"); + "cvcore::Tensor::Tensor ==> Parameter rowPitch error"); } } template::value>::type * = nullptr> - Tensor(std::size_t width, std::size_t height, std::size_t depth, std::size_t channelCount, DataType *dataPtr, - B isCPU = true) + Tensor(std::size_t width, std::size_t height, std::size_t depth, + std::size_t channelCount, DataType * dataPtr, B isCPU = true) : detail::Tensor4D({{depth, height * width * channelCount}, - {height, width * channelCount}, - {width, channelCount}, - {channelCount, 1}}, - dataPtr, isCPU) - { + {height, width * channelCount}, {width, channelCount}, + {channelCount, 1}}, dataPtr, isCPU) { } template::value>::type * = nullptr> - Tensor(std::size_t width, std::size_t height, std::size_t depth, std::size_t channelCount, std::size_t rowPitch, - DataType *dataPtr, B isCPU = true) + Tensor(std::size_t width, std::size_t height, std::size_t depth, + std::size_t channelCount, std::size_t rowPitch, DataType * dataPtr, B isCPU = true) : detail::Tensor4D({{depth, height * rowPitch / GetChannelSize(CT)}, - {height, rowPitch / GetChannelSize(CT)}, - {width, channelCount}, - {channelCount, 1}}, - dataPtr, isCPU) - { - if (rowPitch % GetChannelSize(CT) != 0) - { + {height, rowPitch / GetChannelSize(CT)}, {width, channelCount}, + {channelCount, 1}}, dataPtr, isCPU) { + if (rowPitch % GetChannelSize(CT) != 0) { throw std::domain_error( - "cvcore::Tensor::Tensor ==> Parameter rowPitch is not evenly divisible by channel size"); + "cvcore::Tensor::Tensor ==> Parameter rowPitch error"); } } }; @@ -1051,9 +969,8 @@ class Tensor : public detail::Tensor4D * @tparam CT channel type. */ template -class Tensor : public detail::Tensor4D -{ -public: +class Tensor : public detail::Tensor4D { + public: using DataType = typename detail::ChannelTypeToNative::Type; static constexpr ChannelCount kChannelCount = CC; @@ -1063,71 +980,60 @@ class Tensor : public detail::Tensor4D template::type> Tensor(std::size_t width, std::size_t height, std::size_t depth, bool isCPU = true) : detail::Tensor4D({{depth, detail::ChannelToCount() * width * height}, - {detail::ChannelToCount(), width * height}, - {height, width}, - {width, 1}}, - isCPU) - { + {detail::ChannelToCount(), width * height}, {height, width}, + {width, 1}}, isCPU) { } template::type> - Tensor(std::size_t width, std::size_t height, std::size_t depth, std::size_t channelCount, bool isCPU = true) + Tensor(std::size_t width, std::size_t height, std::size_t depth, + std::size_t channelCount, bool isCPU = true) : detail::Tensor4D( - {{depth, channelCount * width * height}, {channelCount, width * height}, {height, width}, {width, 1}}, - isCPU) - { + {{depth, channelCount * width * height}, {channelCount, width * height}, + {height, width}, {width, 1}}, isCPU) { } template::type> - Tensor(std::size_t width, std::size_t height, std::size_t depth, DataType *dataPtr, bool isCPU = true) + Tensor(std::size_t width, std::size_t height, std::size_t depth, + DataType * dataPtr, bool isCPU = true) : detail::Tensor4D({{depth, detail::ChannelToCount() * width * height}, - {detail::ChannelToCount(), width * height}, - {height, width}, - {width, 1}}, - dataPtr, isCPU) - { + {detail::ChannelToCount(), width * height}, {height, width}, + {width, 1}}, dataPtr, isCPU) { } template::value>::type * = nullptr> - Tensor(std::size_t width, std::size_t height, std::size_t depth, std::size_t rowPitch, DataType *dataPtr, - B isCPU = true) - : detail::Tensor4D({{depth, detail::ChannelToCount() * height * rowPitch / GetChannelSize(CT)}, - {detail::ChannelToCount(), height * rowPitch / GetChannelSize(CT)}, - {height, rowPitch / GetChannelSize(CT)}, - {width, 1}}, - dataPtr, isCPU) - { - if (rowPitch % GetChannelSize(CT) != 0) - { + Tensor(std::size_t width, std::size_t height, std::size_t depth, + std::size_t rowPitch, DataType * dataPtr, B isCPU = true) + : detail::Tensor4D({{depth, + detail::ChannelToCount() * height * rowPitch / GetChannelSize(CT)}, + {detail::ChannelToCount(), height * rowPitch / GetChannelSize(CT)}, + {height, rowPitch / GetChannelSize(CT)}, {width, 1}}, dataPtr, isCPU) { + if (rowPitch % GetChannelSize(CT) != 0) { throw std::domain_error( - "cvcore::Tensor::Tensor ==> Parameter rowPitch is not evenly divisible by channel size"); + "cvcore::Tensor::Tensor ==> Parameter rowPitch error"); } } template::type> - Tensor(std::size_t width, std::size_t height, std::size_t depth, std::size_t channelCount, DataType *dataPtr, - bool isCPU = true) + Tensor(std::size_t width, std::size_t height, std::size_t depth, + std::size_t channelCount, DataType * dataPtr, bool isCPU = true) : detail::Tensor4D( - {{depth, channelCount * width * height}, {channelCount, width * height}, {height, width}, {width, 1}}, - dataPtr, isCPU) - { + {{depth, channelCount * width * height}, {channelCount, width * height}, + {height, width}, {width, 1}}, dataPtr, isCPU) { } template::value>::type * = nullptr> - Tensor(std::size_t width, std::size_t height, std::size_t depth, std::size_t channelCount, std::size_t rowPitch, - DataType *dataPtr, B isCPU = true) - : detail::Tensor4D({{depth, channelCount * height * rowPitch / GetChannelSize(CT)}, - {channelCount, height * rowPitch / GetChannelSize(CT)}, - {height, rowPitch / GetChannelSize(CT)}, - {width, 1}}, - dataPtr, isCPU) - { - if (rowPitch % GetChannelSize(CT) != 0) - { + Tensor(std::size_t width, std::size_t height, std::size_t depth, + std::size_t channelCount, std::size_t rowPitch, DataType * dataPtr, B isCPU = true) + : detail::Tensor4D({{depth, + channelCount * height * rowPitch / GetChannelSize(CT)}, + {channelCount, height * rowPitch / GetChannelSize(CT)}, + {height, rowPitch / GetChannelSize(CT)}, + {width, 1}}, dataPtr, isCPU) { + if (rowPitch % GetChannelSize(CT) != 0) { throw std::domain_error( - "cvcore::Tensor::Tensor ==> Parameter rowPitch is not evenly divisible by channel size"); + "cvcore::Tensor::Tensor ==> Parameter rowPitch error"); } } }; @@ -1138,9 +1044,8 @@ class Tensor : public detail::Tensor4D * @tparam CT channel type. */ template -class Tensor : public detail::Tensor4D -{ -public: +class Tensor : public detail::Tensor4D { + public: using DataType = typename detail::ChannelTypeToNative::Type; static constexpr ChannelCount kChannelCount = CC; @@ -1150,40 +1055,34 @@ class Tensor : public detail::Tensor4D template::type> Tensor(std::size_t width, std::size_t height, std::size_t depth, bool isCPU = true) : detail::Tensor4D({{detail::ChannelToCount(), depth * width * height}, - {depth, width * height}, - {height, width}, - {width, 1}}, - isCPU) - { + {depth, width * height}, {height, width}, + {width, 1}}, isCPU) { } template::type> - Tensor(std::size_t width, std::size_t height, std::size_t depth, std::size_t channelCount, bool isCPU = true) + Tensor(std::size_t width, std::size_t height, std::size_t depth, + std::size_t channelCount, bool isCPU = true) : detail::Tensor4D( - {{channelCount, depth * width * height}, {depth, width * height}, {height, width}, {width, 1}}, isCPU) - { + {{channelCount, depth * width * height}, {depth, width * height}, + {height, width}, {width, 1}}, isCPU) { } template::type> - Tensor(std::size_t width, std::size_t height, std::size_t depth, DataType *dataPtr, bool isCPU = true) - : detail::Tensor4D({{detail::ChannelToCount(), depth * width * height}, - {depth, width * height}, - {height, width}, - {width, 1}}, - dataPtr, isCPU) - { + Tensor(std::size_t width, std::size_t height, std::size_t depth, + DataType * dataPtr, bool isCPU = true) + : detail::Tensor4D({{detail::ChannelToCount(), + depth * width * height}, {depth, width * height}, + {height, width}, {width, 1}}, dataPtr, isCPU) { } template::type> - Tensor(std::size_t width, std::size_t height, std::size_t depth, std::size_t channelCount, DataType *dataPtr, - bool isCPU = true) + Tensor(std::size_t width, std::size_t height, std::size_t depth, + std::size_t channelCount, DataType * dataPtr, bool isCPU = true) : detail::Tensor4D( - {{channelCount, depth * width * height}, {depth, width * height}, {height, width}, {width, 1}}, dataPtr, - isCPU) - { + {{channelCount, depth * width * height}, {depth, width * height}, + {height, width}, {width, 1}}, dataPtr, isCPU) { } }; -} // namespace cvcore - -#endif // CVCORE_TENSOR_H +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/TensorOperators.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/TensorOperators.cpp similarity index 65% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/TensorOperators.cpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/TensorOperators.cpp index 02d34ca..d91b3a4 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/TensorOperators.cpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/TensorOperators.cpp @@ -15,62 +15,49 @@ // // SPDX-License-Identifier: Apache-2.0 -#include "cv/tensor_ops/TensorOperators.h" - #include +#include -#ifdef ENABLE_VPI -#include "vpi/VPITensorOperators.h" -#endif +#include "extensions/tensorops/core/TensorOperators.h" +#include "extensions/tensorops/core/VPITensorOperators.h" -namespace cvcore { namespace tensor_ops { +namespace cvcore { +namespace tensor_ops { typename TensorContextFactory::MultitonType TensorContextFactory::instances; std::mutex TensorContextFactory::instanceMutex; -std::error_code TensorContextFactory::CreateContext(TensorOperatorContext &tensorContext, TensorBackend backend) -{ - using PairType = typename TensorContextFactory::MultitonType::mapped_type; - using CounterType = typename PairType::first_type; - using ValuePtrType = typename PairType::second_type; +std::error_code TensorContextFactory::CreateContext(TensorOperatorContext& tensorContext, + TensorBackend backend) { + using PairType = TensorContextFactory::MultitonType::mapped_type; + using ValuePtrType = PairType::second_type; std::lock_guard instanceLock(instanceMutex); std::error_code result = ErrorCode::SUCCESS; tensorContext = nullptr; - + auto contextItr = instances.find(backend); - if (contextItr == instances.end() && IsBackendSupported(backend)) - { - switch (backend) - { + if (contextItr == instances.end() && IsBackendSupported(backend)) { + switch (backend) { case TensorBackend::VPI: -#ifdef ENABLE_VPI - try - { - instances[backend] = std::make_pair(1, ValuePtrType(new VPITensorContext{})); + try { + instances[backend] = std::make_pair(1, ValuePtrType(new VPITensorContext{})); } - catch (std::error_code &e) - { + catch (std::error_code &e) { result = e; } - catch (...) - { + catch (...) { result = ErrorCode::INVALID_OPERATION; } -#else // _WIN32 - result = ErrorCode::NOT_IMPLEMENTED; -#endif // _WIN32 break; default: result = ErrorCode::NOT_IMPLEMENTED; break; } tensorContext = instances[backend].second.get(); - } - else - { + } else { contextItr->second.first++; tensorContext = contextItr->second.second.get(); } @@ -78,39 +65,34 @@ std::error_code TensorContextFactory::CreateContext(TensorOperatorContext &tenso return result; } -std::error_code TensorContextFactory::DestroyContext(TensorOperatorContext &context) -{ +std::error_code TensorContextFactory::DestroyContext(TensorOperatorContext& context) { std::lock_guard instanceLock(instanceMutex); auto backend = context->Backend(); context = nullptr; auto contextItr = instances.find(backend); - if (contextItr != instances.end()) - { + if (contextItr != instances.end()) { contextItr->second.first--; - if (contextItr->second.first == 0) - { + if (contextItr->second.first == 0) { instances.erase(backend); } } return ErrorCode::SUCCESS; } -bool TensorContextFactory::IsBackendSupported(TensorBackend backend) -{ +bool TensorContextFactory::IsBackendSupported(TensorBackend backend) { bool result = false; - switch (backend) - { - case TensorBackend::VPI: -#ifdef ENABLE_VPI - result = true; -#endif // _WIN32 - break; - default: - break; + switch (backend) { + case TensorBackend::VPI: + result = true; + break; + default: + break; } return result; } -}} // namespace cvcore::tensor_ops + +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/TensorOperators.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/TensorOperators.h similarity index 66% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/TensorOperators.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/TensorOperators.h index e32a3df..5ab89bf 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/tensor_ops/TensorOperators.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/TensorOperators.h @@ -15,34 +15,33 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_TENSOROPERATORS_H -#define CVCORE_TENSOROPERATORS_H +#pragma once #include -#include #include +#include +#include -#include "cv/tensor_ops/ITensorOperatorContext.h" -#include "cv/tensor_ops/ITensorOperatorStream.h" +#include "extensions/tensorops/core/ITensorOperatorContext.h" +#include "extensions/tensorops/core/ITensorOperatorStream.h" -namespace cvcore { namespace tensor_ops { +namespace cvcore { +namespace tensor_ops { -class TensorContextFactory -{ -public: - static std::error_code CreateContext(TensorOperatorContext &, TensorBackend backend); - static std::error_code DestroyContext(TensorOperatorContext &context); +class TensorContextFactory { + public: + static std::error_code CreateContext(TensorOperatorContext&, TensorBackend backend); + static std::error_code DestroyContext(TensorOperatorContext & context); static bool IsBackendSupported(TensorBackend backend); -private: + private: using MultitonType = std::unordered_map>>; + std::pair>>; static MultitonType instances; static std::mutex instanceMutex; }; -}} // namespace cvcore::tensor_ops - -#endif // CVCORE_TENSOROPERATORS_H +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/Traits.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Traits.h similarity index 84% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/Traits.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Traits.h index a78b780..9de82f3 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/include/cv/core/Traits.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/Traits.h @@ -15,61 +15,56 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_TRAITS_H -#define CVCORE_TRAITS_H +#pragma once #include -#include "MathTypes.h" -#include "Tensor.h" +#include "extensions/tensorops/core/MathTypes.h" +#include "extensions/tensorops/core/Tensor.h" -namespace cvcore { namespace traits { +namespace cvcore { +namespace tensor_ops { // ----------------------------------------------------------------------------- // Type Properties // ----------------------------------------------------------------------------- template -struct is_tensor : std::false_type -{ +struct is_tensor : std::false_type { }; template -struct is_tensor> : std::true_type -{ +struct is_tensor> : std::true_type { }; template -struct is_planar : std::false_type -{ +struct is_planar : std::false_type { static_assert(is_tensor::value, ""); }; template -struct is_planar> : std::integral_constant -{ +struct is_planar> : std::integral_constant { }; template -struct is_interleaved : std::false_type -{ +struct is_interleaved : std::false_type { static_assert(is_tensor::value, ""); }; template -struct is_interleaved> : std::integral_constant -{ +struct is_interleaved> : std::integral_constant { }; template -struct is_batch : std::false_type -{ +struct is_batch : std::false_type { static_assert(is_tensor::value, ""); }; template -struct is_batch> : std::integral_constant -{ +struct is_batch> : std::integral_constant { }; // ----------------------------------------------------------------------------- @@ -77,22 +72,19 @@ struct is_batch> : std::integral_constant -struct to_planar -{ +struct to_planar { static_assert(is_tensor::value, ""); using type = TensorType; }; template -struct to_planar> -{ +struct to_planar> { using type = Tensor; }; template -struct to_planar> -{ +struct to_planar> { using type = Tensor; }; @@ -100,28 +92,24 @@ template using to_planar_t = typename to_planar::type; template -struct to_interleaved -{ +struct to_interleaved { static_assert(is_tensor::value, ""); using type = TensorType; }; template -struct to_interleaved> -{ +struct to_interleaved> { using type = Tensor; }; template -struct to_interleaved> -{ +struct to_interleaved> { using type = Tensor; }; template -struct to_interleaved> -{ +struct to_interleaved> { using type = Tensor; }; @@ -129,22 +117,19 @@ template using to_interleaved_t = typename to_interleaved::type; template -struct add_batch -{ +struct add_batch { static_assert(is_tensor::value, ""); using type = TensorType; }; template -struct add_batch> -{ +struct add_batch> { using type = Tensor; }; template -struct add_batch> -{ +struct add_batch> { using type = Tensor; }; @@ -152,28 +137,24 @@ template using add_batch_t = typename add_batch::type; template -struct remove_batch -{ +struct remove_batch { static_assert(is_tensor::value, ""); using type = TensorType; }; template -struct remove_batch> -{ +struct remove_batch> { using type = Tensor; }; template -struct remove_batch> -{ +struct remove_batch> { using type = Tensor; }; template -struct remove_batch> -{ +struct remove_batch> { using type = Tensor; }; @@ -181,14 +162,12 @@ template using remove_batch_t = typename remove_batch::type; template -struct to_c1 -{ +struct to_c1 { static_assert(is_tensor::value, ""); }; template -struct to_c1> -{ +struct to_c1> { using type = Tensor; }; @@ -196,14 +175,12 @@ template using to_c1_t = typename to_c1::type; template -struct to_c2 -{ +struct to_c2 { static_assert(is_tensor::value, ""); }; template -struct to_c2> -{ +struct to_c2> { using type = Tensor; }; @@ -211,14 +188,12 @@ template using to_c2_t = typename to_c2::type; template -struct to_c3 -{ +struct to_c3 { static_assert(is_tensor::value, ""); }; template -struct to_c3> -{ +struct to_c3> { using type = Tensor; }; @@ -226,14 +201,12 @@ template using to_c3_t = typename to_c3::type; template -struct to_c4 -{ +struct to_c4 { static_assert(is_tensor::value, ""); }; template -struct to_c4> -{ +struct to_c4> { using type = Tensor; }; @@ -241,14 +214,12 @@ template using to_c4_t = typename to_c4::type; template -struct to_cx -{ +struct to_cx { static_assert(is_tensor::value, ""); }; template -struct to_cx> -{ +struct to_cx> { using type = Tensor; }; @@ -256,14 +227,12 @@ template using to_cx_t = typename to_cx::type; template -struct to_u8 -{ +struct to_u8 { static_assert(is_tensor::value, ""); }; template -struct to_u8> -{ +struct to_u8> { using type = Tensor; }; @@ -271,14 +240,12 @@ template using to_u8_t = typename to_u8::type; template -struct to_u16 -{ +struct to_u16 { static_assert(is_tensor::value, ""); }; template -struct to_u16> -{ +struct to_u16> { using type = Tensor; }; @@ -286,14 +253,12 @@ template using to_u16_t = typename to_u16::type; template -struct to_f16 -{ +struct to_f16 { static_assert(is_tensor::value, ""); }; template -struct to_f16> -{ +struct to_f16> { using type = Tensor; }; @@ -301,14 +266,12 @@ template using to_f16_t = typename to_f16::type; template -struct to_f32 -{ +struct to_f32 { static_assert(is_tensor::value, ""); }; template -struct to_f32> -{ +struct to_f32> { using type = Tensor; }; @@ -439,40 +402,34 @@ template struct get_dim; template<> -struct get_dim -{ +struct get_dim { static constexpr int value = 1; }; template<> -struct get_dim -{ +struct get_dim { static constexpr int value = 1; }; template<> -struct get_dim -{ +struct get_dim { static constexpr int value = 2; }; template<> -struct get_dim -{ +struct get_dim { static constexpr int value = 2; }; template<> -struct get_dim -{ +struct get_dim { static constexpr int value = 3; }; template<> -struct get_dim -{ +struct get_dim { static constexpr int value = 3; }; -}} // namespace cvcore::traits -#endif // CVCORE_TRAITS_H +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIColorConvertImpl.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIColorConvertImpl.cpp similarity index 51% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIColorConvertImpl.cpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIColorConvertImpl.cpp index 693ca5c..7485bfb 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIColorConvertImpl.cpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIColorConvertImpl.cpp @@ -14,96 +14,77 @@ // limitations under the License. // // SPDX-License-Identifier: Apache-2.0 -#include -#include -#include -#include -#include "cv/core/CameraModel.h" -#include "cv/core/Image.h" -#include "cv/core/Memory.h" -#include "cv/core/ProfileUtils.h" -#include "cv/core/Tensor.h" -#include "cv/tensor_ops/ImageUtils.h" - -#include "VPIColorConvertImpl.h" -#include "VPIEnumMapping.h" -#include "VPIStatusMapping.h" - -// VPI includes +#include #include #include #include -#include -#ifdef NVBENCH_ENABLE -#include -#endif +#include +#include +#include +#include + +#include "extensions/tensorops/core/CameraModel.h" +#include "extensions/tensorops/core/Image.h" +#include "extensions/tensorops/core/ImageUtils.h" +#include "extensions/tensorops/core/Memory.h" +#include "extensions/tensorops/core/Tensor.h" +#include "extensions/tensorops/core/VPIColorConvertImpl.h" +#include "extensions/tensorops/core/VPIEnumMapping.h" +#include "extensions/tensorops/core/VPIStatusMapping.h" -namespace cvcore { namespace tensor_ops { +namespace cvcore { +namespace tensor_ops { VPITensorStream::VPIColorConvertImpl::VPIColorConvertImpl() : m_inputImage(nullptr) - , m_outputImage(nullptr) -{ + , m_outputImage(nullptr) { std::memset(reinterpret_cast(&m_inputImageData), 0, sizeof(VPIImageData)); std::memset(reinterpret_cast(&m_outputImageData), 0, sizeof(VPIImageData)); } template -std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image &outputImage, const Image &inputImage, - VPIStream &stream, VPIBackend backend) -{ +std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image& outputImage, + const Image& inputImage, VPIStream& stream, VPIBackend backend) { std::error_code errCode = make_error_code(VPIStatus::VPI_SUCCESS); bool paramsChanged = m_inputImage == nullptr || m_outputImage == nullptr || CheckParamsChanged(m_inputImageData, inputImage) || CheckParamsChanged(m_outputImageData, outputImage); - if (paramsChanged) - { + if (paramsChanged) { DestroyVPIImageWrapper(m_inputImage, m_inputImageData); DestroyVPIImageWrapper(m_outputImage, m_outputImageData); errCode = CreateVPIImageWrapper(m_inputImage, m_inputImageData, inputImage, backend); - if (errCode == make_error_code(VPI_SUCCESS)) - { + if (errCode == make_error_code(VPI_SUCCESS)) { errCode = CreateVPIImageWrapper(m_outputImage, m_outputImageData, outputImage, backend); } } - if (errCode == make_error_code(VPIStatus::VPI_SUCCESS)) - { + if (errCode == make_error_code(VPIStatus::VPI_SUCCESS)) { errCode = UpdateImage(m_inputImage, m_inputImageData, inputImage); } - if (errCode == make_error_code(VPIStatus::VPI_SUCCESS)) - { + if (errCode == make_error_code(VPIStatus::VPI_SUCCESS)) { errCode = UpdateImage(m_outputImage, m_outputImageData, outputImage); } - if (errCode == make_error_code(VPIStatus::VPI_SUCCESS)) - { -#ifdef NVBENCH_ENABLE - std::string tag = "VPIColorConvert_" + GetMemoryTypeAsString(inputImage.isCPU()) +"Input_" + GetMemoryTypeAsString(outputImage.isCPU()) +"Output_" + getVPIBackendString(backend) + "Backend"; - nv::bench::Timer timerFunc = - nv::bench::VPI(tag.c_str(), nv::bench::Flag::DEFAULT, stream); -#endif - errCode = make_error_code(vpiSubmitConvertImageFormat(stream, backend, m_inputImage, m_outputImage, nullptr)); + if (errCode == make_error_code(VPIStatus::VPI_SUCCESS)) { + errCode = make_error_code(vpiSubmitConvertImageFormat(stream, backend, + m_inputImage, m_outputImage, nullptr)); } - if (errCode == make_error_code(VPIStatus::VPI_SUCCESS)) - { + if (errCode == make_error_code(VPIStatus::VPI_SUCCESS)) { errCode = make_error_code(vpiStreamSync(stream)); } - if (errCode != make_error_code(VPIStatus::VPI_SUCCESS)) - { + if (errCode != make_error_code(VPIStatus::VPI_SUCCESS)) { return errCode; } return make_error_code(ErrorCode::SUCCESS); } -VPITensorStream::VPIColorConvertImpl::~VPIColorConvertImpl() -{ +VPITensorStream::VPIColorConvertImpl::~VPIColorConvertImpl() { // Destroy Input VPIImage DestroyVPIImageWrapper(m_inputImage, m_inputImageData); @@ -111,25 +92,41 @@ VPITensorStream::VPIColorConvertImpl::~VPIColorConvertImpl() DestroyVPIImageWrapper(m_outputImage, m_outputImageData); } -template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image &, const Image &, - VPIStream &, VPIBackend); -template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image &, const Image &, - VPIStream &, VPIBackend); -template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image &, const Image &, - VPIStream &, VPIBackend); -template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image &, const Image &, - VPIStream &, VPIBackend); -template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image &, const Image &, - VPIStream &, VPIBackend); -template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image &, const Image &, - VPIStream &, VPIBackend); -template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image &, const Image &, - VPIStream &, VPIBackend); -template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image &, const Image &, - VPIStream &, VPIBackend); -template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image &, const Image &, - VPIStream &, VPIBackend); -template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image &, const Image &, - VPIStream &, VPIBackend); - -}} // namespace cvcore::tensor_ops +template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image&, + const Image&, VPIStream&, VPIBackend); +template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image&, + const Image&, VPIStream&, VPIBackend); +template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image&, + const Image&, VPIStream&, VPIBackend); +template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image&, + const Image&, VPIStream&, VPIBackend); +template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image&, + const Image&, VPIStream&, VPIBackend); +template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image&, + const Image&, VPIStream&, VPIBackend); +template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image&, + const Image&, VPIStream&, VPIBackend); +template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image&, + const Image&, VPIStream&, VPIBackend); +template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image&, + const Image&, VPIStream&, VPIBackend); +template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image&, + const Image&, VPIStream&, VPIBackend); +template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image&, + const Image&, VPIStream&, VPIBackend); +template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image&, + const Image&, VPIStream&, VPIBackend); +template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image&, + const Image&, VPIStream&, VPIBackend); +template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image&, + const Image&, VPIStream&, VPIBackend); +template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image&, + const Image&, VPIStream&, VPIBackend); +template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image&, + const Image&, VPIStream&, VPIBackend); +template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image&, + const Image&, VPIStream&, VPIBackend); +template std::error_code VPITensorStream::VPIColorConvertImpl::execute(Image&, + const Image&, VPIStream&, VPIBackend); +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIColorConvertImpl.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIColorConvertImpl.h similarity index 72% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIColorConvertImpl.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIColorConvertImpl.h index 0e71266..feede7b 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIColorConvertImpl.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIColorConvertImpl.h @@ -15,23 +15,21 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_VPI_COLOR_CONVERT_IMPL_H -#define CVCORE_VPI_COLOR_CONVERT_IMPL_H - -#include "VPITensorOperators.h" -#include "cv/tensor_ops/ITensorOperatorStream.h" -#include "cv/tensor_ops/ImageUtils.h" +#pragma once #include +#include "extensions/tensorops/core/ImageUtils.h" +#include "extensions/tensorops/core/ITensorOperatorStream.h" +#include "extensions/tensorops/core/VPITensorOperators.h" -namespace cvcore { namespace tensor_ops { +namespace cvcore { +namespace tensor_ops { /** * Color convert implementation for VPI backend. */ -class VPITensorStream::VPIColorConvertImpl -{ -public: +class VPITensorStream::VPIColorConvertImpl { + public: /** * Image color conversion constructor. */ @@ -45,21 +43,20 @@ class VPITensorStream::VPIColorConvertImpl * @param backend specified VPI backend. */ template - std::error_code execute(Image &outputImage, const Image &inputImage, VPIStream &stream, - VPIBackend backend); + std::error_code execute(Image & outputImage, const Image & inputImage, + VPIStream & stream, VPIBackend backend); /** * Image color conversion destroy function to deallocate resources. */ ~VPIColorConvertImpl(); -private: + private: VPIImage m_inputImage; VPIImage m_outputImage; VPIImageData m_inputImageData; VPIImageData m_outputImageData; }; -}} // namespace cvcore::tensor_ops - -#endif //CVCORE_VPI_COLOR_CONVERT_IMPL_H +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIEnumMapping.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIEnumMapping.h similarity index 85% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIEnumMapping.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIEnumMapping.h index d611167..027a9b3 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIEnumMapping.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIEnumMapping.h @@ -15,19 +15,19 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_VPIENUMMAPPING_H -#define CVCORE_VPIENUMMAPPING_H +#pragma once +#include #include -#include "VPITensorOperators.h" +#include "extensions/tensorops/core/VPITensorOperators.h" +#include "vpi/Types.h" -namespace cvcore { namespace tensor_ops { +namespace cvcore { +namespace tensor_ops { -constexpr VPIBackend ToVpiBackendType(const ComputeEngine &computeEngine) -{ - switch (computeEngine) - { +constexpr VPIBackend ToVpiBackendType(const ComputeEngine & computeEngine) { + switch (computeEngine) { case ComputeEngine::CPU: return VPIBackend::VPI_BACKEND_CPU; case ComputeEngine::PVA: @@ -43,12 +43,10 @@ constexpr VPIBackend ToVpiBackendType(const ComputeEngine &computeEngine) } } -constexpr VPIInterpolationType ToVpiInterpolationType(InterpolationType value) -{ +constexpr VPIInterpolationType ToVpiInterpolationType(InterpolationType value) { VPIInterpolationType result = VPI_INTERP_NEAREST; - switch (value) - { + switch (value) { case INTERP_NEAREST: result = VPI_INTERP_NEAREST; break; @@ -65,12 +63,10 @@ constexpr VPIInterpolationType ToVpiInterpolationType(InterpolationType value) return result; } -constexpr VPIBorderExtension ToVpiBorderType(BorderType value) -{ +constexpr VPIBorderExtension ToVpiBorderType(BorderType value) { VPIBorderExtension result = VPI_BORDER_ZERO; - switch (value) - { + switch (value) { case BORDER_ZERO: result = VPI_BORDER_ZERO; break; @@ -90,12 +86,10 @@ constexpr VPIBorderExtension ToVpiBorderType(BorderType value) return result; } -constexpr VPIImageFormat ToVpiImageFormat(ImageType value) -{ +constexpr VPIImageFormat ToVpiImageFormat(ImageType value) { VPIImageFormat result = VPI_IMAGE_FORMAT_Y8_ER; - switch (value) - { + switch (value) { case Y_U8: result = VPI_IMAGE_FORMAT_Y8_ER; break; @@ -133,12 +127,10 @@ constexpr VPIImageFormat ToVpiImageFormat(ImageType value) return result; } -constexpr VPIPixelType ToVpiPixelType(ImageType value) -{ +constexpr VPIPixelType ToVpiPixelType(ImageType value) { VPIPixelType result = VPI_PIXEL_TYPE_U8; - switch (value) - { + switch (value) { case Y_U8: result = VPI_PIXEL_TYPE_U8; break; @@ -170,10 +162,8 @@ constexpr VPIPixelType ToVpiPixelType(ImageType value) return result; } -static inline std::string getVPIBackendString(VPIBackend vpiBackend) -{ - switch (vpiBackend) - { +static inline std::string getVPIBackendString(VPIBackend vpiBackend) { + switch (vpiBackend) { case VPIBackend::VPI_BACKEND_CPU: return "CPU"; case VPIBackend::VPI_BACKEND_CUDA: @@ -191,6 +181,5 @@ static inline std::string getVPIBackendString(VPIBackend vpiBackend) } } -}} // namespace cvcore::tensor_ops - -#endif // CVCORE_VPIENUMMAPPING_H +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIImageWarp.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIImageWarp.h similarity index 75% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIImageWarp.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIImageWarp.h index 71d098b..b5b4ea0 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIImageWarp.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIImageWarp.h @@ -14,24 +14,24 @@ // limitations under the License. // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_VPIIMAGEWARP_H -#define CVCORE_VPIIMAGEWARP_H +#pragma once + +#include #include #include -#include "cv/tensor_ops/IImageWarp.h" -#include "cv/tensor_ops/Errors.h" -#include +#include "extensions/tensorops/core/Errors.h" +#include "extensions/tensorops/core/IImageWarp.h" -namespace cvcore { namespace tensor_ops { -struct VPIImageWarp : public IImageWarp -{ +namespace cvcore { +namespace tensor_ops { + +struct VPIImageWarp : public IImageWarp { ~VPIImageWarp() = default; VPIPayload payload; }; -}} // namespace cvcore::tensor_ops - -#endif // CVCORE_VPIIMAGEWARP_H +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIRemapImpl.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIRemapImpl.cpp similarity index 75% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIRemapImpl.cpp rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIRemapImpl.cpp index cb544fd..ba6dc97 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIRemapImpl.cpp +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIRemapImpl.cpp @@ -15,29 +15,23 @@ // // SPDX-License-Identifier: Apache-2.0 -#include -#include - -#include "VPIRemapImpl.h" -#include "VPIEnumMapping.h" -#include "VPIStatusMapping.h" - -#include "cv/core/CameraModel.h" -#include "cv/core/Image.h" - +#include "extensions/tensorops/core/VPIRemapImpl.h" #include #include -#ifdef NVBENCH_ENABLE -#include -#endif +#include + +#include "extensions/tensorops/core/CameraModel.h" +#include "extensions/tensorops/core/Image.h" +#include "extensions/tensorops/core/VPIEnumMapping.h" +#include "extensions/tensorops/core/VPIStatusMapping.h" -namespace cvcore { namespace tensor_ops { +namespace cvcore { +namespace tensor_ops { VPITensorStream::VPIRemapImpl::VPIRemapImpl() : m_inputImage(nullptr) - , m_outputImage(nullptr) -{ + , m_outputImage(nullptr) { std::memset(reinterpret_cast(&m_inputImageData), 0, sizeof(VPIImageData)); std::memset(reinterpret_cast(&m_outputImageData), 0, sizeof(VPIImageData)); } @@ -45,27 +39,23 @@ VPITensorStream::VPIRemapImpl::VPIRemapImpl() template std::error_code VPITensorStream::VPIRemapImpl::initialize(Image & outImage, const Image & inImage, - VPIBackend backend) -{ + VPIBackend backend) { std::error_code status; status = CreateVPIImageWrapper(m_inputImage, m_inputImageData, inImage, backend); - if(status == make_error_code(VPI_SUCCESS)) - { + if (status == make_error_code(VPI_SUCCESS)) { status = CreateVPIImageWrapper(m_outputImage, m_outputImageData, outImage, backend); } return status; } -template std::error_code VPITensorStream::VPIRemapImpl::initialize(Image & outImage, - const Image & inImage, VPIBackend); -template std::error_code VPITensorStream::VPIRemapImpl::initialize(Image & outImage, - const Image & inImage, VPIBackend); -template std::error_code VPITensorStream::VPIRemapImpl::initialize(Image & outImage, - const Image & inImage, VPIBackend); -template std::error_code VPITensorStream::VPIRemapImpl::initialize(Image & outImage, - const Image & inImage, VPIBackend); - -// ----------------------------------------------------------------------------- +template std::error_code VPITensorStream::VPIRemapImpl::initialize(Image& outImage, + const Image& inImage, VPIBackend); +template std::error_code VPITensorStream::VPIRemapImpl::initialize(Image& outImage, + const Image& inImage, VPIBackend); +template std::error_code VPITensorStream::VPIRemapImpl::initialize(Image& outImage, + const Image& inImage, VPIBackend); +template std::error_code VPITensorStream::VPIRemapImpl::initialize(Image& outImage, + const Image& inImage, VPIBackend); template std::error_code VPITensorStream::VPIRemapImpl::execute(Image & outImage, @@ -74,8 +64,7 @@ std::error_code VPITensorStream::VPIRemapImpl::execute(Image & outImage, InterpolationType interpolation, BorderType border, VPIStream & stream, - VPIBackend backend) -{ + VPIBackend backend) { std::error_code status = make_error_code(VPI_SUCCESS); VPIInterpolationType vpiInterpolationType = ToVpiInterpolationType(interpolation); VPIBorderExtension vpiBorderExt = ToVpiBorderType(border); @@ -84,37 +73,27 @@ std::error_code VPITensorStream::VPIRemapImpl::execute(Image & outImage, CheckParamsChanged(m_inputImageData, inImage) || CheckParamsChanged(m_outputImageData, outImage); - if(paramsChanged) - { + if (paramsChanged) { DestroyVPIImageWrapper(m_inputImage, m_inputImageData); DestroyVPIImageWrapper(m_outputImage, m_outputImageData); status = initialize(outImage, inImage, backend); } - if(status == make_error_code(VPI_SUCCESS)) - { + if (status == make_error_code(VPI_SUCCESS)) { status = UpdateImage(m_inputImage, m_inputImageData, inImage); } - if(status == make_error_code(VPI_SUCCESS)) - { + if (status == make_error_code(VPI_SUCCESS)) { status = UpdateImage(m_outputImage, m_outputImageData, outImage); } - if(status == make_error_code(VPI_SUCCESS)) - { -#ifdef NVBENCH_ENABLE - std::string tag = "VPISubmitRemap_" + GetMemoryTypeAsString(inImage.isCPU()) +"Input_" + GetMemoryTypeAsString(outImage.isCPU()) +"Output_" + getVPIBackendString(backend) + "Backend"; - nv::bench::Timer timerFunc = - nv::bench::VPI(tag.c_str(), nv::bench::Flag::DEFAULT, stream); -#endif + if (status == make_error_code(VPI_SUCCESS)) { // Submit remap task for Lens Distortion Correction status = make_error_code(vpiSubmitRemap(stream, backend, warp->payload, - m_inputImage, m_outputImage, vpiInterpolationType, vpiBorderExt, 0)); + m_inputImage, m_outputImage, vpiInterpolationType, vpiBorderExt, 0)); } - if(status == make_error_code(VPI_SUCCESS)) - { + if (status == make_error_code(VPI_SUCCESS)) { // Wait for remap to complete status = make_error_code(vpiStreamSync(stream)); } @@ -148,13 +127,11 @@ template std::error_code VPITensorStream::VPIRemapImpl::execute(Image & ou BorderType border, VPIStream & stream, VPIBackend backend); -// ----------------------------------------------------------------------------- -VPITensorStream::VPIRemapImpl::~VPIRemapImpl() -{ +VPITensorStream::VPIRemapImpl::~VPIRemapImpl() { DestroyVPIImageWrapper(m_inputImage, m_inputImageData); DestroyVPIImageWrapper(m_outputImage, m_outputImageData); } -// ----------------------------------------------------------------------------- -}} // namespace cvcore::tensor_ops +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIRemapImpl.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIRemapImpl.h new file mode 100644 index 0000000..e3f64a4 --- /dev/null +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIRemapImpl.h @@ -0,0 +1,79 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include "extensions/tensorops/core/VPIImageWarp.h" +#include "extensions/tensorops/core/VPITensorOperators.h" +namespace cvcore { +namespace tensor_ops { + +/** + * Remap implementation used for Lens Distortion. + */ +class VPITensorStream::VPIRemapImpl { + public: + /* VPIRemapImpl constructor */ + VPIRemapImpl(); + + /** + * Remap Intialization. + * @param outputImage Remap output image of Type + * @param inputImage Remap input image of Type + * @param backend Compute backend + * @return Success if intialization is done successfully, otherwise error is returned + */ + template + std::error_code initialize(Image & outImage, + const Image & inImage, + VPIBackend backend); + + /** + * Remap execution function(non-blocking) + * Application is reqiured to call Sync() before accessing the generated Image. + * @param outImage Remap output image of type NV12 + * @param inImage Remap input image of type NV12 + * @param warp Remap warp pointer + * @param interpolation Interpolation type used for remap + * @param border Border type used for remap + * @param stream VPI stream used for remap + * @param backend VPI backend used for remap + * @return Success if remap is submitted successfully, otherwise error is returned + */ + template + std::error_code execute(Image & outImage, + const Image & inImage, + const VPIImageWarp * warp, + InterpolationType interpolation, + BorderType border, + VPIStream & stream, + VPIBackend backend); + + /* VPIRemapImpl destructor to release resources */ + ~VPIRemapImpl(); + + private: + VPIImage m_inputImage; + VPIImage m_outputImage; + VPIImageData m_inputImageData; + VPIImageData m_outputImageData; +}; + +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIResizeImpl.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIResizeImpl.cpp new file mode 100644 index 0000000..bde11b4 --- /dev/null +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIResizeImpl.cpp @@ -0,0 +1,124 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 +#include "extensions/tensorops/core/VPIResizeImpl.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "extensions/tensorops/core/CameraModel.h" +#include "extensions/tensorops/core/Image.h" +#include "extensions/tensorops/core/VPIEnumMapping.h" +#include "extensions/tensorops/core/VPIStatusMapping.h" + +namespace cvcore { +namespace tensor_ops { + +template +std::error_code VPITensorStream::VPIResizeImpl::execute(Image& outputImage, + const Image& inputImage, InterpolationType interpolation, BorderType border, + VPIStream& stream, VPIBackend backend) { + std::error_code errCode = ErrorCode::SUCCESS; + VPIStatus status = VPIStatus::VPI_SUCCESS; + VPIInterpolationType interpType; + VPIBorderExtension borderExt; + interpType = ToVpiInterpolationType(interpolation); + borderExt = ToVpiBorderType(border); + + bool paramsChanged = m_inputImage == nullptr || m_outputImage == nullptr || + CheckParamsChanged(m_inputImageData, inputImage) || + CheckParamsChanged(m_outputImageData, outputImage); + if (paramsChanged) { + DestroyVPIImageWrapper(m_inputImage, m_inputImageData); + DestroyVPIImageWrapper(m_outputImage, m_outputImageData); + errCode = CreateVPIImageWrapper(m_inputImage, m_inputImageData, + inputImage, backend); + if (errCode == make_error_code(VPI_SUCCESS)) { + errCode = CreateVPIImageWrapper(m_outputImage, m_outputImageData, + outputImage, backend); + } + } else { + errCode = UpdateImage(m_inputImage, m_inputImageData, inputImage); + if (errCode == make_error_code(VPIStatus::VPI_SUCCESS)) { + errCode = UpdateImage(m_outputImage, m_outputImageData, outputImage); + } + } + + if (status == VPIStatus::VPI_SUCCESS) { + status = vpiSubmitRescale(stream, backend, m_inputImage, m_outputImage, + interpType, borderExt, 0); + } + + if (status == VPIStatus::VPI_SUCCESS) { + status = vpiStreamSync(stream); + } + + if (status != VPIStatus::VPI_SUCCESS) { + return make_error_code(status); + } + return make_error_code(ErrorCode::SUCCESS); +} + +VPITensorStream::VPIResizeImpl::VPIResizeImpl() + : m_inputImage(nullptr) + , m_outputImage(nullptr) { + std::memset(reinterpret_cast(&m_inputImageData), 0, sizeof(VPIImageData)); + std::memset(reinterpret_cast(&m_outputImageData), 0, sizeof(VPIImageData)); +} + +/** +* Image resizing destroy function to deallocate resources. +*/ +VPITensorStream::VPIResizeImpl::~VPIResizeImpl() { + // Destroy Input VPIImage + DestroyVPIImageWrapper(m_inputImage, m_inputImageData); + // Destroy Output VPIImage + DestroyVPIImageWrapper(m_outputImage, m_outputImageData); +} + +template std::error_code VPITensorStream::VPIResizeImpl::execute(Image& outputImage, + const Image& inputImage, InterpolationType interpolation, BorderType border, + VPIStream& stream, VPIBackend backend); +template std::error_code VPITensorStream::VPIResizeImpl::execute(Image& outputImage, + const Image& inputImage, + InterpolationType interpolation, BorderType border, + VPIStream& stream, VPIBackend backend); +template std::error_code VPITensorStream::VPIResizeImpl::execute(Image& outputImage, + const Image& inputImage, + InterpolationType interpolation, BorderType border, + VPIStream& stream, VPIBackend backend); +template std::error_code VPITensorStream::VPIResizeImpl::execute(Image& outputImage, + const Image& inputImage, + InterpolationType interpolation, BorderType border, + VPIStream& stream, VPIBackend backend); +template std::error_code VPITensorStream::VPIResizeImpl::execute(Image& outputImage, + const Image& inputImage, + InterpolationType interpolation, BorderType border, + VPIStream& stream, VPIBackend backend); +template std::error_code VPITensorStream::VPIResizeImpl::execute(Image& outputImage, + const Image& inputImage, + InterpolationType interpolation, BorderType border, + VPIStream& stream, VPIBackend backend); + +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIResizeImpl.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIResizeImpl.h similarity index 72% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIResizeImpl.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIResizeImpl.h index ec20a45..dc0c1b9 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIResizeImpl.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIResizeImpl.h @@ -15,24 +15,21 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_VPIRESIZEIMPL_H -#define CVCORE_VPIRESIZEIMPL_H - -#include "VPITensorOperators.h" +#pragma once #include +#include "extensions/tensorops/core/ImageUtils.h" +#include "extensions/tensorops/core/ITensorOperatorStream.h" +#include "extensions/tensorops/core/VPITensorOperators.h" -#include "cv/tensor_ops/ITensorOperatorStream.h" -#include "cv/tensor_ops/ImageUtils.h" - -namespace cvcore { namespace tensor_ops { +namespace cvcore { +namespace tensor_ops { /** * Remap implementation used for Lens Distortion. */ -class VPITensorStream::VPIResizeImpl -{ -public: +class VPITensorStream::VPIResizeImpl { + public: /** * Initialization for Image resizing. */ @@ -46,21 +43,21 @@ class VPITensorStream::VPIResizeImpl * @param border Image border extension used for resize */ template - std::error_code execute(Image &outputImage, const Image &inputImage, InterpolationType interpolation, - BorderType border, VPIStream &stream, VPIBackend backend); + std::error_code execute(Image & outputImage, const Image & inputImage, + InterpolationType interpolation, BorderType border, + VPIStream & stream, VPIBackend backend); /** * Image resizing destroy function to deallocate resources. */ ~VPIResizeImpl(); -private: + private: VPIImage m_inputImage; VPIImage m_outputImage; VPIImageData m_inputImageData; VPIImageData m_outputImageData; }; -}} // namespace cvcore::tensor_ops - -#endif //CVCORE_VPIRESIZEIMPL_H +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIStatusMapping.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIStatusMapping.cpp new file mode 100644 index 0000000..2c3076d --- /dev/null +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIStatusMapping.cpp @@ -0,0 +1,118 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 + +#include "extensions/tensorops/core/VPIStatusMapping.h" + +#include +#include "extensions/tensorops/core/Errors.h" +#include "vpi/Status.h" + +#ifndef __cpp_lib_to_underlying +// Using a C++23 feature by hacking std +namespace std { + template + constexpr underlying_type_t to_underlying(Enum e) noexcept { + return static_cast>(e); + } +}; +#endif // __cpp_lib_to_underlying + +namespace cvcore { +namespace tensor_ops { + +namespace detail { +struct VPIStatusCategory : std::error_category { + const char * name() const noexcept final { + return "vpi-status"; + } + + std::string message(int value) const final { + std::string result = "VPI Status"; + + return result; + } + + std::error_condition default_error_condition(int code) const noexcept final { + std::error_condition result; + + switch (code) { + case VPI_SUCCESS: + result = ErrorCode::SUCCESS; + break; + + case VPI_ERROR_INVALID_ARGUMENT: + result = ErrorCode::INVALID_ARGUMENT; + break; + + case VPI_ERROR_INVALID_IMAGE_FORMAT: + result = ErrorCode::INVALID_IMAGE_FORMAT; + break; + + case VPI_ERROR_INVALID_ARRAY_TYPE: + result = ErrorCode::INVALID_STORAGE_TYPE; + break; + + case VPI_ERROR_INVALID_PAYLOAD_TYPE: + result = ErrorCode::INVALID_STORAGE_TYPE; + break; + + case VPI_ERROR_INVALID_OPERATION: + result = ErrorCode::INVALID_OPERATION; + break; + + case VPI_ERROR_INVALID_CONTEXT: + result = ErrorCode::INVALID_ENGINE_TYPE; + break; + + case VPI_ERROR_DEVICE: + result = ErrorCode::DEVICE_ERROR; + break; + + case VPI_ERROR_NOT_READY: + result = ErrorCode::NOT_READY; + break; + + case VPI_ERROR_BUFFER_LOCKED: + result = ErrorCode::SYSTEM_ERROR; + break; + + case VPI_ERROR_OUT_OF_MEMORY: + result = ErrorCode::OUT_OF_MEMORY; + break; + + case VPI_ERROR_INTERNAL: + result = ErrorCode::SYSTEM_ERROR; + break; + + default: + result = ErrorCode::NOT_IMPLEMENTED; + break; + } + + return result; + } +}; +} // namespace detail + +const detail::VPIStatusCategory errorCategory{}; + +std::error_code make_error_code(VPIStatus ec) noexcept { + return {static_cast(std::to_underlying(ec)), errorCategory}; +} + +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIStatusMapping.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIStatusMapping.h similarity index 80% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIStatusMapping.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIStatusMapping.h index 961d4fc..43fa6fe 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIStatusMapping.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIStatusMapping.h @@ -15,10 +15,9 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_VPISTATUSMAPPING_H -#define CVCORE_VPISTATUSMAPPING_H +#pragma once -#include "cv/core/CVError.h" +#include "extensions/tensorops/core/CVError.h" #include "vpi/Status.h" // WARNING: Extending base C++ namespace to cover cvcore error codes @@ -27,12 +26,12 @@ namespace std { template <> struct is_error_code_enum : true_type {}; -} // namespace std +} // namespace std -namespace cvcore { namespace tensor_ops { +namespace cvcore { +namespace tensor_ops { std::error_code make_error_code(VPIStatus) noexcept; -}} // namespace cvcore::tensor_ops - -#endif //CVCORE_VPISTATUSMAPPING_H +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIStereoDisparityEstimatorImpl.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIStereoDisparityEstimatorImpl.cpp new file mode 100644 index 0000000..86f42a4 --- /dev/null +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIStereoDisparityEstimatorImpl.cpp @@ -0,0 +1,187 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 + +#include "extensions/tensorops/core/VPIStereoDisparityEstimatorImpl.h" +#include +#include +#include "extensions/tensorops/core/CameraModel.h" +#include "extensions/tensorops/core/Image.h" +#include "extensions/tensorops/core/VPIEnumMapping.h" +#include "extensions/tensorops/core/VPIStatusMapping.h" + +namespace cvcore { +namespace tensor_ops { + +VPITensorStream::VPIStereoDisparityEstimatorImpl::VPIStereoDisparityEstimatorImpl() + : m_inputLeftImage(nullptr) + , m_inputRightImage(nullptr) + , m_outputImage(nullptr) + , m_tempImage(nullptr) + , m_payload(nullptr) + , m_stereoParams() { + std::memset(reinterpret_cast(&m_inputLeftImageData), 0, sizeof(VPIImageData)); + std::memset(reinterpret_cast(&m_inputRightImageData), 0, sizeof(VPIImageData)); + std::memset(reinterpret_cast(&m_outputImageData), 0, sizeof(VPIImageData)); + // Disparity values returned from VPI are in Q10.5 format, i.e., signed fixed point + // with 5 fractional bits. Divide it by 32.0f to convert it to floating point. + vpiInitConvertImageFormatParams(&m_cvtParams); + m_cvtParams.scale = 1.0f / 32; +} + +template +std::error_code VPITensorStream::VPIStereoDisparityEstimatorImpl::initialize( + Image& outImage, + const Image& leftImage, + const Image& rightImage, + VPIBackend backend) { + std::error_code status; + const std::error_code success = make_error_code(VPI_SUCCESS); + status = CreateVPIImageWrapper(m_inputLeftImage, + m_inputLeftImageData, leftImage, backend); + if (status == success) { + status = CreateVPIImageWrapper(m_inputRightImage, m_inputRightImageData, + rightImage, backend); + } + if (status == success) { + status = CreateVPIImageWrapper(m_outputImage, m_outputImageData, + outImage, backend); + } + if (status == success) { + status = make_error_code( + vpiImageCreate(outImage.getWidth(), outImage.getHeight(), + VPI_IMAGE_FORMAT_S16, 0, &m_tempImage)); + } + if (status == success) { + status = make_error_code(vpiCreateStereoDisparityEstimator(backend, + outImage.getWidth(), outImage.getHeight(), + ToVpiImageFormat(T_IN), NULL, &m_payload)); + } + return status; +} + +template std::error_code VPITensorStream::VPIStereoDisparityEstimatorImpl::initialize( + Image& outImage, + const Image& leftImage, + const Image& rightImage, + VPIBackend backend); +template std::error_code VPITensorStream::VPIStereoDisparityEstimatorImpl::initialize( + Image& outImage, + const Image& leftImage, + const Image& rightImage, + VPIBackend backend); +template std::error_code VPITensorStream::VPIStereoDisparityEstimatorImpl::initialize( + Image& outImage, + const Image& leftImage, + const Image& rightImage, + VPIBackend backend); + +template +std::error_code VPITensorStream::VPIStereoDisparityEstimatorImpl::execute( + Image& outImage, + const Image& leftImage, + const Image& rightImage, + size_t windowSize, size_t maxDisparity, + VPIStream& stream, VPIBackend backend) { + std::error_code status = make_error_code(VPI_SUCCESS); + m_stereoParams.windowSize = static_cast(windowSize); + m_stereoParams.maxDisparity = static_cast(maxDisparity); + + bool paramsChanged = m_inputLeftImage == nullptr || + m_inputRightImage == nullptr || + m_outputImage == nullptr || + CheckParamsChanged(m_inputLeftImageData, leftImage) || + CheckParamsChanged(m_inputRightImageData, rightImage) || + CheckParamsChanged(m_outputImageData, outImage); + + if (paramsChanged) { + if (m_payload != nullptr) { + vpiPayloadDestroy(m_payload); + } + if (m_tempImage != nullptr) { + vpiImageDestroy(m_tempImage); + } + DestroyVPIImageWrapper(m_inputLeftImage, m_inputLeftImageData); + DestroyVPIImageWrapper(m_inputRightImage, m_inputRightImageData); + DestroyVPIImageWrapper(m_outputImage, m_outputImageData); + + status = initialize(outImage, leftImage, rightImage, backend); + } + + if (status == make_error_code(VPI_SUCCESS)) { + status = UpdateImage(m_inputLeftImage, m_inputLeftImageData, leftImage); + } + + if (status == make_error_code(VPI_SUCCESS)) { + status = UpdateImage(m_inputRightImage, m_inputRightImageData, rightImage); + } + + if (status == make_error_code(VPI_SUCCESS)) { + status = UpdateImage(m_outputImage, m_outputImageData, outImage); + } + + if (status == make_error_code(VPI_SUCCESS)) { + // Submit SGM task for Stereo Disparity Estimator + status = make_error_code(vpiSubmitStereoDisparityEstimator( + stream, backend, m_payload, m_inputLeftImage, m_inputRightImage, + m_tempImage, NULL, &m_stereoParams)); + } + + if (status == make_error_code(VPI_SUCCESS)) { + // Submit SGM task for Stereo Disparity Estimator + status = + make_error_code(vpiSubmitConvertImageFormat(stream, backend, + m_tempImage, m_outputImage, &m_cvtParams)); + } + + if (status == make_error_code(VPI_SUCCESS)) { + // Wait for stereo disparity estimator to complete + status = make_error_code(vpiStreamSync(stream)); + } + + if (status != make_error_code(VPI_SUCCESS)) { + return status; + } + return make_error_code(ErrorCode::SUCCESS); +} + +template std::error_code VPITensorStream::VPIStereoDisparityEstimatorImpl::execute( + Image& outImage, const Image& leftImage, + const Image& rightImage, size_t windowSize, + size_t maxDisparity, VPIStream& stream, VPIBackend backend); +template std::error_code VPITensorStream::VPIStereoDisparityEstimatorImpl::execute( + Image& outImage, const Image& leftImage, + const Image& rightImage, size_t windowSize, + size_t maxDisparity, VPIStream& stream, VPIBackend backend); +template std::error_code VPITensorStream::VPIStereoDisparityEstimatorImpl::execute( + Image& outImage, const Image& leftImage, + const Image& rightImage, size_t windowSize, + size_t maxDisparity, VPIStream& stream, VPIBackend backend); + +VPITensorStream::VPIStereoDisparityEstimatorImpl::~VPIStereoDisparityEstimatorImpl() { + if (m_payload != nullptr) { + vpiPayloadDestroy(m_payload); + } + if (m_tempImage != nullptr) { + vpiImageDestroy(m_tempImage); + } + DestroyVPIImageWrapper(m_inputLeftImage, m_inputLeftImageData); + DestroyVPIImageWrapper(m_inputRightImage, m_inputRightImageData); + DestroyVPIImageWrapper(m_outputImage, m_outputImageData); +} + +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIStereoDisparityEstimatorImpl.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIStereoDisparityEstimatorImpl.h similarity index 57% rename from isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIStereoDisparityEstimatorImpl.h rename to isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIStereoDisparityEstimatorImpl.h index 53a5c63..8ff196d 100644 --- a/isaac_ros_image_proc/gxf/tensorops/cvcore/src/tensor_ops/vpi/VPIStereoDisparityEstimatorImpl.h +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPIStereoDisparityEstimatorImpl.h @@ -15,22 +15,21 @@ // // SPDX-License-Identifier: Apache-2.0 -#ifndef CVCORE_VPISTEREODISPARITYESTIMATORIMPL_H -#define CVCORE_VPISTEREODISPARITYESTIMATORIMPL_H +#pragma once -#include #include #include +#include +#include "extensions/tensorops/core/VPITensorOperators.h" -#include "VPITensorOperators.h" +namespace cvcore { +namespace tensor_ops { -namespace cvcore { namespace tensor_ops { /** * StereoDisparityEstimator implementation used for stereo dispaity estimate. */ -class VPITensorStream::VPIStereoDisparityEstimatorImpl -{ -public: +class VPITensorStream::VPIStereoDisparityEstimatorImpl { + public: /* VPIStereoDisparityEstimatorImpl constructor */ VPIStereoDisparityEstimatorImpl(); @@ -43,29 +42,32 @@ class VPITensorStream::VPIStereoDisparityEstimatorImpl * @return Success if intialization is done successfully, otherwise error is returned */ template - std::error_code initialize(Image &outImage, const Image &leftImage, const Image &rightImage, - VPIBackend backend); + std::error_code initialize(Image & outImage, const Image & leftImage, + const Image & rightImage, VPIBackend backend); /** - * StereoDisparityEstimator execution function(non-blocking) - * Application is reqiured to call Sync() before accessing the generated Image. - * @param outImage StereoDisparityEstimator output image - * @param leftImage StereoDisparityEstimator input left image - * @param rightImage StereoDisparityEstimator input right image - * @param windowSize Represents the median filter size (on PVA+NVENC_VIC backend) or census transform window size (other backends) used in the algorithm - * @param maxDisparity Maximum disparity for matching search - * @param stream VPI stream used for StereoDisparityEstimator - * @param backend VPI backend used for StereoDisparityEstimator - * @return Success if StereoDisparityEstimator is submitted successfully, otherwise error is returned - */ + * StereoDisparityEstimator execution function(non-blocking) + * Application is reqiured to call Sync() before accessing the generated Image. + * @param outImage StereoDisparityEstimator output image + * @param leftImage StereoDisparityEstimator input left image + * @param rightImage StereoDisparityEstimator input right image + * @param windowSize Represents the median filter size (on PVA+NVENC_VIC backend) + * or census transform window size (other backends) used in the algorithm + * @param maxDisparity Maximum disparity for matching search + * @param stream VPI stream used for StereoDisparityEstimator + * @param backend VPI backend used for StereoDisparityEstimator + * @return Success if StereoDisparityEstimator is submitted successfully, + * otherwise error is returned + */ template - std::error_code execute(Image &outImage, const Image &leftImage, const Image &rightImage, - size_t windowSize, size_t maxDisparity, VPIStream &stream, VPIBackend backend); + std::error_code execute(Image & outImage, const Image & leftImage, + const Image & rightImage, size_t windowSize, size_t maxDisparity, + VPIStream & stream, VPIBackend backend); /* VPIStereoDisparityEstimatorImpl destructor to release resources */ ~VPIStereoDisparityEstimatorImpl(); -private: + private: VPIImage m_inputLeftImage; VPIImage m_inputRightImage; VPIImage m_outputImage; @@ -78,6 +80,5 @@ class VPITensorStream::VPIStereoDisparityEstimatorImpl VPIConvertImageFormatParams m_cvtParams; }; -}} // namespace cvcore::tensor_ops - -#endif //CVCORE_VPISTEREODISPARITYESTIMATORIMPL_H +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPITensorOperators.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPITensorOperators.cpp new file mode 100644 index 0000000..fb8be4e --- /dev/null +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPITensorOperators.cpp @@ -0,0 +1,554 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include +#include + +#include +#include + +#include "extensions/tensorops/core/CameraModel.h" +#include "extensions/tensorops/core/Image.h" +#include "extensions/tensorops/core/VPIColorConvertImpl.h" +#include "extensions/tensorops/core/VPIEnumMapping.h" +#include "extensions/tensorops/core/VPIImageWarp.h" +#include "extensions/tensorops/core/VPIRemapImpl.h" +#include "extensions/tensorops/core/VPIResizeImpl.h" +#include "extensions/tensorops/core/VPIStatusMapping.h" +#include "extensions/tensorops/core/VPIStereoDisparityEstimatorImpl.h" +#include "extensions/tensorops/core/VPITensorOperators.h" + +namespace cvcore { +namespace tensor_ops { + +namespace detail { + +// helper function to wrap VPI image for NV12 / NV24 image types +template::value>::type * = nullptr> +std::error_code CreateVPIImageWrapperImpl(VPIImage& vpiImg, VPIImageData& imgdata, + const Image& cvcoreImage, VPIBackend backend) { + std::memset(reinterpret_cast(&imgdata), 0, sizeof(VPIImageData)); + + imgdata.bufferType = cvcoreImage.isCPU() ? VPI_IMAGE_BUFFER_HOST_PITCH_LINEAR + : VPI_IMAGE_BUFFER_CUDA_PITCH_LINEAR; + imgdata.buffer.pitch.format = ToVpiImageFormat(T); + imgdata.buffer.pitch.numPlanes = 2; + imgdata.buffer.pitch.planes[0].data = const_cast(cvcoreImage.getLumaData()); + imgdata.buffer.pitch.planes[0].height = cvcoreImage.getLumaHeight(); + imgdata.buffer.pitch.planes[0].width = cvcoreImage.getLumaWidth(); + imgdata.buffer.pitch.planes[0].pixelType = VPI_PIXEL_TYPE_U8; + imgdata.buffer.pitch.planes[0].pitchBytes = cvcoreImage.getLumaStride( + TensorDimension::HEIGHT) * sizeof(uint8_t); + imgdata.buffer.pitch.planes[1].data = const_cast(cvcoreImage.getChromaData()); + imgdata.buffer.pitch.planes[1].height = cvcoreImage.getChromaHeight(); + imgdata.buffer.pitch.planes[1].width = cvcoreImage.getChromaWidth(); + imgdata.buffer.pitch.planes[1].pixelType = VPI_PIXEL_TYPE_2U8; + imgdata.buffer.pitch.planes[1].pitchBytes = cvcoreImage.getChromaStride( + TensorDimension::HEIGHT) * sizeof(uint8_t); + VPIStatus vpiStatus; + vpiStatus = vpiImageCreateWrapper(&imgdata, nullptr, backend, &vpiImg); + + return make_error_code(vpiStatus); +} + +// helper function to wrap VPI image for interleaved image types +template::value>::type * = nullptr> +std::error_code CreateVPIImageWrapperImpl(VPIImage& vpiImg, VPIImageData& imgdata, + const Image& cvcoreImage, VPIBackend backend) { + std::memset(reinterpret_cast(&imgdata), 0, sizeof(VPIImageData)); + + using D = typename Image::DataType; + imgdata.bufferType = cvcoreImage.isCPU() ? VPI_IMAGE_BUFFER_HOST_PITCH_LINEAR + : VPI_IMAGE_BUFFER_CUDA_PITCH_LINEAR; + imgdata.buffer.pitch.format = ToVpiImageFormat(T); + imgdata.buffer.pitch.numPlanes = 1; + imgdata.buffer.pitch.planes[0].data = const_cast(cvcoreImage.getData()); + imgdata.buffer.pitch.planes[0].height = cvcoreImage.getHeight(); + imgdata.buffer.pitch.planes[0].width = cvcoreImage.getWidth(); + imgdata.buffer.pitch.planes[0].pixelType = ToVpiPixelType(T); + imgdata.buffer.pitch.planes[0].pitchBytes = cvcoreImage.getStride( + TensorDimension::HEIGHT) * GetImageElementSize(T); + VPIStatus vpiStatus; + vpiStatus = vpiImageCreateWrapper(&imgdata, nullptr, backend, &vpiImg); + + return make_error_code(vpiStatus); +} + +// helper function to wrap VPI image for planar image types (TODO: not supported in vpi so far) +template::value>::type * = nullptr> +std::error_code CreateVPIImageWrapperImpl(VPIImage& vpiImg, VPIImageData& imgdata, + const Image& cvcoreImage, VPIBackend backend) { + return make_error_code(VPI_ERROR_INVALID_IMAGE_FORMAT); +} + +} // namespace detail + +std::error_code VPITensorContext::CreateStream(TensorOperatorStream& tensorStream, + const ComputeEngine& computeEngine) { + tensorStream = nullptr; + + if (!IsComputeEngineCompatible(computeEngine)) { + return ErrorCode::INVALID_ENGINE_TYPE; + } + + try { + tensorStream = new VPITensorStream(computeEngine); + } + catch (std::error_code &e) { + return e; + } + catch (...) { + return ErrorCode::INVALID_OPERATION; + } + + return ErrorCode::SUCCESS; +} + +VPITensorStream::VPITensorStream(const ComputeEngine& computeEngine) + : m_resizer(new VPIResizeImpl()) + , m_remapper(new VPIRemapImpl()) + , m_colorConverter(new VPIColorConvertImpl()) + , m_stereoDisparityEstimator(new VPIStereoDisparityEstimatorImpl()) { + VPIBackend backend = ToVpiBackendType(computeEngine); + VPIStatus status = vpiStreamCreate(backend, &m_stream); + if (status != VPI_SUCCESS) { + throw make_error_code(status); + } + m_backend = backend; +} + +VPITensorStream::~VPITensorStream() { + vpiStreamDestroy(m_stream); +} + +std::error_code VPITensorContext::DestroyStream(TensorOperatorStream& inputStream) { + if (inputStream != nullptr) { + delete inputStream; + inputStream = nullptr; + } + return ErrorCode::SUCCESS; +} + +bool VPITensorContext::IsComputeEngineCompatible( + const ComputeEngine& computeEngine) const noexcept { + VPIBackend vpibackend = ToVpiBackendType(computeEngine); + if (vpibackend == VPIBackend::VPI_BACKEND_INVALID) { + return false; + } + return true; +} + +template +std::error_code CreateVPIImageWrapper(VPIImage& vpiImg, VPIImageData& imgdata, + const Image& cvcoreImage, VPIBackend backend) { + return detail::CreateVPIImageWrapperImpl(vpiImg, imgdata, cvcoreImage, backend); +} + +template std::error_code CreateVPIImageWrapper(VPIImage&, + VPIImageData&, const Image&, VPIBackend); +template std::error_code CreateVPIImageWrapper(VPIImage&, + VPIImageData&, const Image&, VPIBackend); +template std::error_code CreateVPIImageWrapper(VPIImage&, + VPIImageData&, const Image&, VPIBackend); +template std::error_code CreateVPIImageWrapper(VPIImage&, + VPIImageData&, const Image&, VPIBackend); +template std::error_code CreateVPIImageWrapper(VPIImage&, + VPIImageData&, const Image&, VPIBackend); +template std::error_code CreateVPIImageWrapper(VPIImage&, + VPIImageData&, const Image& , VPIBackend); +template std::error_code CreateVPIImageWrapper(VPIImage&, + VPIImageData&, const Image&, VPIBackend); +template std::error_code CreateVPIImageWrapper(VPIImage&, + VPIImageData&, const Image&, VPIBackend); +template std::error_code CreateVPIImageWrapper(VPIImage&, + VPIImageData&, const Image&, VPIBackend); +template std::error_code CreateVPIImageWrapper(VPIImage&, + VPIImageData&, const Image&, VPIBackend); + +std::error_code UpdateVPIImageWrapper(VPIImage& image, VPIImageData& imageWrap, bool isCPU) { + VPIStatus status = VPI_SUCCESS; + status = vpiImageSetWrapper(image, &imageWrap); + return make_error_code(status); +} + +std::error_code DestroyVPIImageWrapper(VPIImage& image, VPIImageData& imageWrap) { + std::memset(reinterpret_cast(&imageWrap), 0, sizeof(VPIImageData)); + if (image != nullptr) { + vpiImageDestroy(image); + } + + image = nullptr; + + return ErrorCode::SUCCESS; +} +std::error_code VPITensorStream::Status() noexcept { + return ErrorCode::SUCCESS; +} + +std::error_code VPITensorStream::Resize(Image& outputImage, + const Image& inputImage, InterpolationType interpolation, BorderType border) { + std::unique_lock scopedLock{m_fence}; + std::error_code err_code; + err_code = m_resizer->execute(outputImage, inputImage, interpolation, + border, m_stream, m_backend); + return err_code; +} + +std::error_code VPITensorStream::Resize(Image& outputImage, const Image& inputImage, + InterpolationType interpolation, BorderType border) { + std::unique_lock scopedLock{m_fence}; + std::error_code err_code; + err_code = m_resizer->execute(outputImage, inputImage, interpolation, + border, m_stream, m_backend); + return err_code; +} + +std::error_code VPITensorStream::Resize(Image& outputImage, + const Image& inputImage, InterpolationType interpolation, BorderType border) { + std::unique_lock scopedLock{m_fence}; + std::error_code err_code; + err_code = m_resizer->execute(outputImage, inputImage, + interpolation, border, m_stream, m_backend); + return err_code; +} + +std::error_code VPITensorStream::Resize(Image& outputImage, + const Image& inputImage, InterpolationType interpolation, BorderType border) { + std::unique_lock scopedLock{m_fence}; + + std::error_code err_code; + err_code = m_resizer->execute(outputImage, inputImage, interpolation, + border, m_stream, m_backend); + return err_code; +} + +std::error_code VPITensorStream::Resize(Image& outputImage, + const Image& inputImage, InterpolationType interpolation, BorderType border) { + std::unique_lock scopedLock{m_fence}; + std::error_code err_code; + err_code = m_resizer->execute(outputImage, inputImage, + interpolation, border, m_stream, m_backend); + return err_code; +} + +std::error_code VPITensorStream::Resize(Image& outputImage, + const Image& inputImage, InterpolationType interpolation, BorderType border) { + std::unique_lock scopedLock{m_fence}; + std::error_code err_code; + err_code = m_resizer->execute(outputImage, inputImage, + interpolation, border, m_stream, m_backend); + return err_code; +} + +std::error_code VPITensorStream::Remap(Image& outputImage, + const Image& inputImage, const ImageWarp warp, + InterpolationType interpolation, BorderType border) { + std::unique_lock scopedLock{m_fence}; + return m_remapper->execute(outputImage, inputImage, + reinterpret_cast(warp), + interpolation, border, m_stream, m_backend); +} + +std::error_code VPITensorStream::Remap(Image& outputImage, + const Image& inputImage, const ImageWarp warp, + InterpolationType interpolation, BorderType border) { + std::unique_lock scopedLock{m_fence}; + return m_remapper->execute(outputImage, inputImage, + reinterpret_cast(warp), + interpolation, border, m_stream, m_backend); +} + +std::error_code VPITensorStream::Remap(Image& outputImage, const Image& inputImage, + const ImageWarp warp, InterpolationType interpolation, BorderType border) { + std::unique_lock scopedLock{m_fence}; + return m_remapper->execute(outputImage, inputImage, + reinterpret_cast(warp), + interpolation, border, m_stream, m_backend); +} + +std::error_code VPITensorStream::Remap(Image& outputImage, const Image& inputImage, + const ImageWarp warp, InterpolationType interpolation, BorderType border) { + std::unique_lock scopedLock{m_fence}; + return m_remapper->execute(outputImage, inputImage, + reinterpret_cast(warp), + interpolation, border, m_stream, m_backend); +} + +std::error_code VPITensorStream::ColorConvert(Image& outputImage, + const Image& inputImage) { + std::unique_lock scopedLock{m_fence}; + return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); +} + +std::error_code VPITensorStream::ColorConvert(Image& outputImage, + const Image& inputImage) { + std::unique_lock scopedLock{m_fence}; + return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); +} + +std::error_code VPITensorStream::ColorConvert(Image& outputImage, + const Image& inputImage) { + std::unique_lock scopedLock{m_fence}; + return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); +} + +std::error_code VPITensorStream::ColorConvert(Image& outputImage, + const Image& inputImage) { + std::unique_lock scopedLock{m_fence}; + return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); +} + +std::error_code VPITensorStream::ColorConvert(Image& outputImage, + const Image& inputImage) { + std::unique_lock scopedLock{m_fence}; + return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); +} + +std::error_code VPITensorStream::ColorConvert(Image& outputImage, + const Image& inputImage) { + std::unique_lock scopedLock{m_fence}; + + return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); +} + +std::error_code VPITensorStream::ColorConvert(Image& outputImage, + const Image& inputImage) { + std::unique_lock scopedLock{m_fence}; + return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); +} + +std::error_code VPITensorStream::ColorConvert(Image& outputImage, + const Image& inputImage) { + std::unique_lock scopedLock{m_fence}; + return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); +} + +std::error_code VPITensorStream::ColorConvert(Image& outputImage, + const Image& inputImage) { + std::unique_lock scopedLock{m_fence}; + return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); +} + +std::error_code VPITensorStream::ColorConvert(Image& outputImage, + const Image& inputImage) { + std::unique_lock scopedLock{m_fence}; + return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); +} + +std::error_code VPITensorStream::ColorConvert(Image& outputImage, + const Image& inputImage) { + std::unique_lock scopedLock{m_fence}; + return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); +} + +std::error_code VPITensorStream::ColorConvert(Image& outputImage, + const Image& inputImage) { + std::unique_lock scopedLock{m_fence}; + return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); +} + +std::error_code VPITensorStream::ColorConvert(Image& outputImage, + const Image& inputImage) { + std::unique_lock scopedLock{m_fence}; + return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); +} + +std::error_code VPITensorStream::ColorConvert(Image& outputImage, + const Image& inputImage) { + std::unique_lock scopedLock{m_fence}; + return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); +} + +std::error_code VPITensorStream::ColorConvert(Image& outputImage, + const Image& inputImage) { + std::unique_lock scopedLock{m_fence}; + return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); +} + +std::error_code VPITensorStream::ColorConvert(Image& outputImage, + const Image& inputImage) { + std::unique_lock scopedLock{m_fence}; + return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); +} + +std::error_code VPITensorStream::ColorConvert(Image& outputImage, + const Image& inputImage) { + std::unique_lock scopedLock{m_fence}; + return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); +} + +std::error_code VPITensorStream::ColorConvert(Image& outputImage, + const Image& inputImage) { + std::unique_lock scopedLock{m_fence}; + return m_colorConverter->execute(outputImage, inputImage, m_stream, m_backend); +} + +std::error_code VPITensorStream::StereoDisparityEstimator(Image& outputImage, + const Image& inputLeftImage, const Image& inputRightImage, + size_t windowSize, size_t maxDisparity) { + std::unique_lock scopedLock{m_fence}; + return m_stereoDisparityEstimator->execute(outputImage, inputLeftImage, + inputRightImage, windowSize, maxDisparity, m_stream, m_backend); +} + +std::error_code VPITensorStream::StereoDisparityEstimator(Image& outputImage, + const Image& inputLeftImage, const Image& inputRightImage, + size_t windowSize, size_t maxDisparity) { + std::unique_lock scopedLock{m_fence}; + return m_stereoDisparityEstimator->execute(outputImage, inputLeftImage, + inputRightImage, windowSize, maxDisparity, + m_stream, m_backend); +} + +std::error_code VPITensorStream::StereoDisparityEstimator(Image& outputImage, + const Image& inputLeftImage, const Image& inputRightImage, + size_t windowSize, size_t maxDisparity) { + std::unique_lock scopedLock{m_fence}; + return m_stereoDisparityEstimator->execute(outputImage, inputLeftImage, + inputRightImage, windowSize, maxDisparity, m_stream, m_backend); +} + +TensorBackend VPITensorContext::Backend() const noexcept { + return TensorBackend::VPI; +} + +std::error_code VPITensorStream::GenerateWarpFromCameraModel(ImageWarp& warp, + const ImageGrid& grid, const CameraModel& source, const CameraIntrinsics& target) { + std::unique_lock scopedLock{m_fence}; + VPIStatus status = VPI_SUCCESS; + + VPIWarpMap map = {0}; + map.grid.numHorizRegions = grid.numHorizRegions; + for (std::size_t i = 0; i < static_cast(grid.numHorizRegions); i++) { + map.grid.regionWidth[i] = grid.regionWidth[i]; + map.grid.horizInterval[i] = grid.horizInterval[i]; + } + map.grid.numVertRegions = grid.numVertRegions; + for (std::size_t i = 0; i < static_cast(grid.numVertRegions); i++) { + map.grid.regionHeight[i] = grid.regionHeight[i]; + map.grid.vertInterval[i] = grid.vertInterval[i]; + } + status = vpiWarpMapAllocData(&map); + + if ((status == VPI_SUCCESS) && (map.keypoints)) { + switch (source.distortion.type) { + case CameraDistortionType::Polynomial: { + VPIPolynomialLensDistortionModel distortion; + distortion.k1 = source.distortion.k1; + distortion.k2 = source.distortion.k2; + distortion.k3 = source.distortion.k3; + distortion.k4 = source.distortion.k4; + distortion.k5 = source.distortion.k5; + distortion.k6 = source.distortion.k6; + distortion.p1 = source.distortion.p1; + distortion.p2 = source.distortion.p2; + status = vpiWarpMapGenerateFromPolynomialLensDistortionModel( + source.intrinsic.m_intrinsics, source.extrinsic.m_extrinsics, + target.m_intrinsics, &distortion, &map); + break; + } + case CameraDistortionType::FisheyeEquidistant: { + VPIFisheyeLensDistortionModel distortion; + distortion.k1 = source.distortion.k1; + distortion.k2 = source.distortion.k2; + distortion.k3 = source.distortion.k3; + distortion.k4 = source.distortion.k4; + distortion.mapping = VPI_FISHEYE_EQUIDISTANT; + status = vpiWarpMapGenerateFromFisheyeLensDistortionModel( + source.intrinsic.m_intrinsics, source.extrinsic.m_extrinsics, + target.m_intrinsics, &distortion, &map); + break; + } + case CameraDistortionType::FisheyeEquisolid: { + VPIFisheyeLensDistortionModel distortion; + distortion.k1 = source.distortion.k1; + distortion.k2 = source.distortion.k2; + distortion.k3 = source.distortion.k3; + distortion.k4 = source.distortion.k4; + distortion.mapping = VPI_FISHEYE_EQUISOLID; + status = vpiWarpMapGenerateFromFisheyeLensDistortionModel( + source.intrinsic.m_intrinsics, source.extrinsic.m_extrinsics, + target.m_intrinsics, &distortion, &map); + break; + } + case CameraDistortionType::FisheyeOrthoGraphic: { + VPIFisheyeLensDistortionModel distortion; + distortion.k1 = source.distortion.k1; + distortion.k2 = source.distortion.k2; + distortion.k3 = source.distortion.k3; + distortion.k4 = source.distortion.k4; + distortion.mapping = VPI_FISHEYE_ORTHOGRAPHIC; + status = vpiWarpMapGenerateFromFisheyeLensDistortionModel( + source.intrinsic.m_intrinsics, source.extrinsic.m_extrinsics, + target.m_intrinsics, &distortion, &map); + break; + } + case CameraDistortionType::FisheyeStereographic: { + VPIFisheyeLensDistortionModel distortion; + distortion.k1 = source.distortion.k1; + distortion.k2 = source.distortion.k2; + distortion.k3 = source.distortion.k3; + distortion.k4 = source.distortion.k4; + distortion.mapping = VPI_FISHEYE_STEREOGRAPHIC; + status = vpiWarpMapGenerateFromFisheyeLensDistortionModel( + source.intrinsic.m_intrinsics, source.extrinsic.m_extrinsics, + target.m_intrinsics, &distortion, &map); + break; + } + default: + status = VPI_ERROR_INVALID_ARGUMENT; + break; + } + } + + if ((status == VPI_SUCCESS) && (map.keypoints)) { + if (warp != nullptr) { + vpiPayloadDestroy(reinterpret_cast(warp)->payload); + delete warp; + } + warp = new VPIImageWarp; + status = vpiCreateRemap(m_backend, &map, + &(reinterpret_cast(warp)->payload)); + } + + // Delete map after payload is generated + vpiWarpMapFreeData(&map); + + return make_error_code(status); +} + +std::error_code VPITensorStream::DestroyWarp(ImageWarp& warp) noexcept { + std::unique_lock scopedLock{m_fence}; + if (warp != nullptr) { + try { + vpiPayloadDestroy(reinterpret_cast(warp)->payload); + } + catch (std::error_code &e) { + return e; + } + + delete reinterpret_cast(warp); + warp = nullptr; + } + return make_error_code(ErrorCode::SUCCESS); +} + +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPITensorOperators.h b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPITensorOperators.h new file mode 100644 index 0000000..0284fc8 --- /dev/null +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/core/VPITensorOperators.h @@ -0,0 +1,305 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include "extensions/tensorops/core/CameraModel.h" +#include "extensions/tensorops/core/ITensorOperatorContext.h" +#include "extensions/tensorops/core/ITensorOperatorStream.h" +#include "extensions/tensorops/core/VPIEnumMapping.h" +#include "vpi/CUDAInterop.h" +#include "vpi/Image.h" +#include "vpi/Stream.h" +#include "vpi/Types.h" + +namespace cvcore { +namespace tensor_ops { + +/** + * Returns the corresponding VPI backend given the cvcore compute engine. + * @param computeEngine Compute engine used. + * @return Returns the VPIbackend. + */ +VPIBackend getVPIBackend(const ComputeEngine & computeEngine); + +/** + * Wraps a CVCore Image type into a VPIImage + * @param vpiImage VPIImage + * @param imgdata VPIImage data + * @param cvcoreImage CVCore image + * @param backend Compute backend + * @return error code + */ +template +std::error_code CreateVPIImageWrapper(VPIImage & vpiImg, VPIImageData & imgdata, + const Image & cvcoreImage, VPIBackend backend); + +/** + * Update VPI Image data pointer + * @param vpiImage VPIImage + * @param imgdata VPIImage data + * @param isCPU data is on CPU or GPU + * @return error code + */ +std::error_code UpdateVPIImageWrapper(VPIImage & image, VPIImageData & imageWrap, bool isCPU); + +/** + * Destory Wrapped VPI Image + * @param vpiImage VPIImage + * @param imgdata VPIImage data + * @return error code + */ +std::error_code DestroyVPIImageWrapper(VPIImage & image, VPIImageData & imageWrap); + +/** + * Update VPI Image given CVCORE Image + * @param vpiImage VPIImage + * @param vpiImageData VPIImage data + * @param image CVCORE Image + * @return error code + */ +template::value>::type * = nullptr> +std::error_code UpdateImage(VPIImage & vpiImage, VPIImageData & vpiImageData, + const Image & image) { + using D = typename Image::DataType; + vpiImageData.buffer.pitch.planes[0].data = const_cast(image.getData()); + return UpdateVPIImageWrapper(vpiImage, vpiImageData, image.isCPU()); +} + +/** + * Update VPI Image given CVCORE Image + * @param vpiImage VPIImage + * @param vpiImageData VPIImage data + * @param image CVCORE Image + * @return error code + */ +template::value>::type * = nullptr> +std::error_code UpdateImage(VPIImage & vpiImage, VPIImageData & vpiImageData, + const Image & image) { + vpiImageData.buffer.pitch.planes[0].data = const_cast(image.getLumaData()); + vpiImageData.buffer.pitch.planes[1].data = const_cast(image.getChromaData()); + return UpdateVPIImageWrapper(vpiImage, vpiImageData, image.isCPU()); +} + +/** + * Check if params of VPIImageData is consistent with the given CVCORE Image + * @param vpiImageData VPIImage data + * @param image CVCORE Image + * @return whether param changed + */ +template::value && + !IsPlanarImage::value>::type * = nullptr> +bool CheckParamsChanged(VPIImageData & vpiImageData, const Image & image) { + bool paramsChanged = false; + // Did format change + paramsChanged = paramsChanged || vpiImageData.buffer.pitch.format != ToVpiImageFormat(T); + // Did image dimension change + paramsChanged = + paramsChanged || (vpiImageData.buffer.pitch.planes[0].height != + static_cast(image.getHeight()) || + vpiImageData.buffer.pitch.planes[0].width != + static_cast(image.getWidth())); + return paramsChanged; +} + +/** + * Check if params of VPIImageData is consistent with the given CVCORE Image + * @param vpiImageData VPIImage data + * @param image CVCORE Image + * @return whether param changed + */ +template::value>::type * = nullptr> +bool CheckParamsChanged(VPIImageData & vpiImageData, const Image & image) { + bool paramsChanged = false; + + // Did format change + paramsChanged = paramsChanged || vpiImageData.buffer.pitch.format != ToVpiImageFormat(T); + + // Did image dimension change + paramsChanged = paramsChanged || + (vpiImageData.buffer.pitch.planes[0].height != + static_cast(image.getLumaHeight()) || + vpiImageData.buffer.pitch.planes[0].width != + static_cast(image.getLumaWidth()) || + vpiImageData.buffer.pitch.planes[1].height != + static_cast(image.getChromaHeight()) || + vpiImageData.buffer.pitch.planes[1].width != + static_cast(image.getChromaWidth())); + return paramsChanged; +} + +/** + * Implementation of VPITensorContext + */ +class VPITensorContext : public ITensorOperatorContext { + public: + /** + * Default Constructor for VPI Context. + */ + VPITensorContext() = default; + + /** + * Default Destructor for VPI Context. + */ + ~VPITensorContext() = default; + + /** + * Creates a stream based on compute engine + * @param computeEngine CVCore Compute engine + * @return Pointer to ITensorOperatorStream object. + */ + std::error_code CreateStream(TensorOperatorStream&, + const ComputeEngine & computeEngine) override; + + /** + * Destroy stream creates. + * @param inputStream Input stream to be deleted + * @return Error code + */ + std::error_code DestroyStream(TensorOperatorStream & inputStream) override; + + /** + * Checks if stream type is supported for a given backend. + * @param computeEngine CVCore Compute engine + * @return true if stream type is available. + */ + bool IsComputeEngineCompatible(const ComputeEngine & computeEngine) const noexcept override; + + /** + * Returns the backend type + */ + TensorBackend Backend() const noexcept override; + + private: +}; + +/** + * Implementation of VPITensorStream + */ +class VPITensorStream : public ITensorOperatorStream { + public: + std::error_code Status() noexcept override; + + std::error_code GenerateWarpFromCameraModel(ImageWarp & warp, const ImageGrid & grid, + const CameraModel & source, const CameraIntrinsics & target) override; + std::error_code DestroyWarp(ImageWarp & warp) noexcept override; + + std::error_code Remap(Image & outputImage, const Image & inputImage, + const ImageWarp warp, InterpolationType interpolation, BorderType border) override; + + std::error_code Remap(Image & outputImage, const Image & inputImage, + const ImageWarp warp, InterpolationType interpolation, BorderType border) override; + + std::error_code Remap(Image & outputImage, const Image & inputImage, + const ImageWarp warp, InterpolationType interpolation, BorderType border) override; + + std::error_code Remap(Image & outputImage, const Image & inputImage, + const ImageWarp warp, InterpolationType interpolation, BorderType border) override; + + std::error_code Resize(Image & outputImage, const Image & inputImage, + InterpolationType interpolation, BorderType border) override; + std::error_code Resize(Image & outputImage, const Image & inputImage, + InterpolationType interpolation, BorderType border) override; + std::error_code Resize(Image & outputImage, const Image & inputImage, + InterpolationType interpolation, BorderType border) override; + std::error_code Resize(Image & outputImage, const Image & inputImage, + InterpolationType interpolation, BorderType border) override; + std::error_code Resize(Image & outputImage, const Image & inputImage, + InterpolationType interpolation, BorderType border) override; + std::error_code Resize(Image & outputImage, const Image & inputImage, + InterpolationType interpolation, BorderType border) override; + + std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) override; + std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) override; + std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) override; + std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) override; + std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) override; + std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) override; + std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) override; + std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) override; + std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) override; + std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) override; + std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) override; + std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) override; + std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) override; + std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) override; + std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) override; + std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) override; + std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) override; + std::error_code ColorConvert(Image & outputImage, + const Image & inputImage) override; + + std::error_code StereoDisparityEstimator(Image & outputImage, + const Image & inputLeftImage, + const Image & inputRightImage, size_t windowSize, + size_t maxDisparity) override; + std::error_code StereoDisparityEstimator(Image & outputImage, + const Image & inputLeftImage, + const Image & inputRightImage, size_t windowSize, + size_t maxDisparity) override; + std::error_code StereoDisparityEstimator(Image & outputImage, + const Image & inputLeftImage, + const Image & inputRightImage, size_t windowSize, + size_t maxDisparity) override; + + protected: + friend class VPITensorContext; + + VPITensorStream(const ComputeEngine & computeEngine); + ~VPITensorStream(); + + private: + class VPIResizeImpl; + class VPIRemapImpl; + class VPIColorConvertImpl; + class VPIStereoDisparityEstimatorImpl; + + mutable std::mutex m_fence; + + std::unique_ptr m_resizer; + std::unique_ptr m_remapper; + std::unique_ptr m_colorConverter; + std::unique_ptr m_stereoDisparityEstimator; + + VPIStream m_stream; + VPIBackend m_backend; +}; + +} // namespace tensor_ops +} // namespace cvcore diff --git a/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/tensorops.cpp b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/tensorops.cpp new file mode 100644 index 0000000..18ba2be --- /dev/null +++ b/isaac_ros_image_proc/gxf/tensorops/extensions/tensorops/tensorops.cpp @@ -0,0 +1,87 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 +#include "extensions/tensorops/components/CameraModel.hpp" +#include "extensions/tensorops/components/ConvertColorFormat.hpp" +#include "extensions/tensorops/components/CropAndResize.hpp" +#include "extensions/tensorops/components/Frame3D.hpp" +#include "extensions/tensorops/components/ImageAdapter.hpp" +#include "extensions/tensorops/components/InterleavedToPlanar.hpp" +#include "extensions/tensorops/components/Normalize.hpp" +#include "extensions/tensorops/components/Reshape.hpp" +#include "extensions/tensorops/components/Resize.hpp" +#include "extensions/tensorops/components/TensorOperator.hpp" +#include "extensions/tensorops/components/TensorStream.hpp" +#include "extensions/tensorops/components/Undistort.hpp" +#include "gxf/std/extension_factory_helper.hpp" + +GXF_EXT_FACTORY_BEGIN() +GXF_EXT_FACTORY_SET_INFO(0x6eae64ff97a94d9b, 0xb324f85e6a98a75a, "NvCvTensorOpsExtension", + "Generic CVCORE tensor_ops interfaces", + "Nvidia_Gxf", "3.2.2", "LICENSE"); + +GXF_EXT_FACTORY_ADD(0xd073a92344ba4b81, 0xbd0f18f4996048e9, + nvidia::isaac::tensor_ops::CameraModel, nvidia::gxf::Component, + "Construct Camera distortion model / Camera intrinsic compatible with CVCORE"); + +GXF_EXT_FACTORY_ADD(0x6c9419223e4b4c2b, 0x899a4d65279c6507, + nvidia::isaac::tensor_ops::Frame3D, nvidia::gxf::Component, + "Construct Camera extrinsic compatible with CVCORE"); + +GXF_EXT_FACTORY_ADD(0xd94385e5b35b4634, 0x9adb0d214a3865f6, + nvidia::isaac::tensor_ops::TensorStream, nvidia::gxf::Component, + "Wrapper of CVCORE ITensorOperatorStream/ITensorOperatorContext"); + +GXF_EXT_FACTORY_ADD(0xd0c4ddad486a4a91, 0xb69c8a5304b205ef, + nvidia::isaac::tensor_ops::ImageAdapter, nvidia::gxf::Component, + "Utility component for conversion between message and cvcore image type"); + +GXF_EXT_FACTORY_ADD(0xadebc792bd0b4a56, 0x99c1405fd2ea0727, + nvidia::isaac::tensor_ops::StreamUndistort, nvidia::gxf::Codelet, + "Codelet for stream image undistortion in tensor_ops"); + +GXF_EXT_FACTORY_ADD(0xa58141ac7eca4ea5, 0x9b545446fe379a11, + nvidia::isaac::tensor_ops::Resize, nvidia::gxf::Codelet, + "Codelet for image resizing in tensor_ops"); + +GXF_EXT_FACTORY_ADD(0xeb8b5f5b36d44b48, 0x81f959fd28e6f677, + nvidia::isaac::tensor_ops::StreamResize, nvidia::gxf::Codelet, + "Codelet for stream image resizing in tensor_ops"); + +GXF_EXT_FACTORY_ADD(0x4a7ff422de3841bc, 0x9e743ac10d9294b6, + nvidia::isaac::tensor_ops::CropAndResize, nvidia::gxf::Codelet, + "Codelet for crop and resizing operation in tensor_ops"); + +GXF_EXT_FACTORY_ADD(0x7018f0b9034c462b, 0xa9fbaf7ee012974f, + nvidia::isaac::tensor_ops::Normalize, nvidia::gxf::Codelet, + "Codelet for image normalization in tensor_ops"); + +GXF_EXT_FACTORY_ADD(0x269d4237f3c3479d, 0xbcca9ecc44c71a70, + nvidia::isaac::tensor_ops::InterleavedToPlanar, nvidia::gxf::Codelet, + "Codelet for convert interleaved image to planar image in tensor_ops"); + +GXF_EXT_FACTORY_ADD(0xfc4d7b4d8fcc4daa, 0xa286056e0fcafa78, + nvidia::isaac::tensor_ops::ConvertColorFormat, nvidia::gxf::Codelet, + "Codelet for image color conversion in tensor_ops"); + +GXF_EXT_FACTORY_ADD(0x5ab4a4d8f7a34552, 0xa90be52660b076fd, + nvidia::isaac::tensor_ops::StreamConvertColorFormat, nvidia::gxf::Codelet, + "Codelet for stream image color conversion in tensor_ops"); + +GXF_EXT_FACTORY_ADD(0x26789b7d5a8d4e84, 0x86b845ec5f4cd12a, + nvidia::isaac::tensor_ops::Reshape, nvidia::gxf::Codelet, + "Codelet for image reshape in tensor_ops"); +GXF_EXT_FACTORY_END() diff --git a/isaac_ros_image_proc/gxf/tensorops/gems/video_buffer/allocator.hpp b/isaac_ros_image_proc/gxf/tensorops/gems/video_buffer/allocator.hpp new file mode 100644 index 0000000..f62c4a0 --- /dev/null +++ b/isaac_ros_image_proc/gxf/tensorops/gems/video_buffer/allocator.hpp @@ -0,0 +1,260 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 +#pragma once + +#include +#include +#include "gxf/multimedia/video.hpp" +#include "gxf/std/allocator.hpp" + +namespace nvidia { +namespace isaac { + +template +struct NoPaddingColorPlanes {}; + +template <> +struct NoPaddingColorPlanes { + explicit NoPaddingColorPlanes(uint32_t width) : planes({gxf::ColorPlane("RGB", 3, width * 3)}) {} + std::array planes; +}; + +template <> +struct NoPaddingColorPlanes { + explicit NoPaddingColorPlanes(uint32_t width) + : planes({nvidia::gxf::ColorPlane("BGR", 3, width * 3)}) {} + std::array planes; +}; + +template <> +struct NoPaddingColorPlanes { + explicit NoPaddingColorPlanes(uint32_t width) + : planes({nvidia::gxf::ColorPlane("RGBA", 4, width * 4)}) {} + std::array planes; +}; + +template <> +struct NoPaddingColorPlanes { + explicit NoPaddingColorPlanes(uint32_t width) + : planes({nvidia::gxf::ColorPlane("BGRA", 4, width * 4)}) {} + std::array planes; +}; + +template <> +struct NoPaddingColorPlanes { + explicit NoPaddingColorPlanes(uint32_t width) + : planes({nvidia::gxf::ColorPlane("RGB", 6, width * 6)}) {} + std::array planes; +}; + +template <> +struct NoPaddingColorPlanes { + explicit NoPaddingColorPlanes(uint32_t width) + : planes({nvidia::gxf::ColorPlane("BGR", 6, width * 6)}) {} + std::array planes; +}; + +template <> +struct NoPaddingColorPlanes { + explicit NoPaddingColorPlanes(uint32_t width) + : planes({nvidia::gxf::ColorPlane("RGB", 12, width * 12)}) {} + std::array planes; +}; + +template <> +struct NoPaddingColorPlanes { + explicit NoPaddingColorPlanes(uint32_t width) + : planes({nvidia::gxf::ColorPlane("BGR", 12, width * 12)}) {} + std::array planes; +}; + +template <> +struct NoPaddingColorPlanes { + explicit NoPaddingColorPlanes(uint32_t width) + : planes({nvidia::gxf::ColorPlane("gray", 1, width)}) {} + std::array planes; +}; + +template <> +struct NoPaddingColorPlanes { + explicit NoPaddingColorPlanes(uint32_t width) + : planes({nvidia::gxf::ColorPlane("gray", 2, width * 2)}) {} + std::array planes; +}; + +template <> +struct NoPaddingColorPlanes { + explicit NoPaddingColorPlanes(uint32_t width) + : planes({nvidia::gxf::ColorPlane("gray", 4, width * 4)}) {} + std::array planes; +}; + +template <> +struct NoPaddingColorPlanes { + explicit NoPaddingColorPlanes(uint32_t width) + : planes({nvidia::gxf::ColorPlane("gray", 4, width * 4)}) {} + std::array planes; +}; + +template <> +struct NoPaddingColorPlanes { + explicit NoPaddingColorPlanes(uint32_t width) + : planes({nvidia::gxf::ColorPlane("Y", 1, width), + nvidia::gxf::ColorPlane("UV", 2, width)}) {} + std::array planes; +}; + +template <> +struct NoPaddingColorPlanes { + explicit NoPaddingColorPlanes(uint32_t width) + : planes({nvidia::gxf::ColorPlane("Y", 1, width), + nvidia::gxf::ColorPlane("UV", 2, width)}) {} + std::array planes; +}; + +template <> +struct NoPaddingColorPlanes { + explicit NoPaddingColorPlanes(uint32_t width) + : planes({nvidia::gxf::ColorPlane("Y", 1, width), + nvidia::gxf::ColorPlane("UV", 2, width * 2)}) {} + std::array planes; +}; + +template <> +struct NoPaddingColorPlanes { + explicit NoPaddingColorPlanes(uint32_t width) + : planes({nvidia::gxf::ColorPlane("Y", 1, width), + nvidia::gxf::ColorPlane("UV", 2, width * 2)}) {} + std::array planes; +}; + +template<> +struct NoPaddingColorPlanes { + explicit NoPaddingColorPlanes(size_t width) + : planes({gxf::ColorPlane("R", 1, width), gxf::ColorPlane("G", 1, width), + gxf::ColorPlane("B", 1, width)}) {} + std::array planes; +}; + +template<> +struct NoPaddingColorPlanes { + explicit NoPaddingColorPlanes(size_t width) + : planes({gxf::ColorPlane("R", 2, width * 2), gxf::ColorPlane("G", 2, width * 2), + gxf::ColorPlane("B", 2, width * 2)}) {} + std::array planes; +}; + +template<> +struct NoPaddingColorPlanes { + explicit NoPaddingColorPlanes(size_t width) + : planes( + {gxf::ColorPlane("R", 4, width * 4), gxf::ColorPlane("G", 4, width * 4), + gxf::ColorPlane("B", 4, width * 4)}) {} + std::array planes; +}; + +template<> +struct NoPaddingColorPlanes { + explicit NoPaddingColorPlanes(size_t width) + : planes({gxf::ColorPlane("B", 1, width), gxf::ColorPlane("G", 1, width), + gxf::ColorPlane("R", 1, width)}) {} + std::array planes; +}; + +template<> +struct NoPaddingColorPlanes { + explicit NoPaddingColorPlanes(size_t width) + : planes( + {gxf::ColorPlane("R", 2, width * 2), gxf::ColorPlane("G", 2, width * 2), + gxf::ColorPlane("B", 2, width * 2)}) {} + std::array planes; +}; + +template<> +struct NoPaddingColorPlanes { + explicit NoPaddingColorPlanes(size_t width) + : planes( + {gxf::ColorPlane("B", 4, width * 4), gxf::ColorPlane("G", 4, width * 4), + gxf::ColorPlane("R", 4, width * 4)}) {} + std::array planes; +}; + +// This includes the list of video buffer formats that supported for the allocator +constexpr bool IsSupportedVideoFormat(const gxf::VideoFormat format) { + return format == gxf::VideoFormat::GXF_VIDEO_FORMAT_RGB || + format == gxf::VideoFormat::GXF_VIDEO_FORMAT_BGR || + format == gxf::VideoFormat::GXF_VIDEO_FORMAT_RGBA || + format == gxf::VideoFormat::GXF_VIDEO_FORMAT_BGRA || + format == gxf::VideoFormat::GXF_VIDEO_FORMAT_RGB16 || + format == gxf::VideoFormat::GXF_VIDEO_FORMAT_BGR16 || + format == gxf::VideoFormat::GXF_VIDEO_FORMAT_RGB32 || + format == gxf::VideoFormat::GXF_VIDEO_FORMAT_BGR32 || + format == gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY || + format == gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY16 || + format == gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY32 || + format == gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY32F || + format == gxf::VideoFormat::GXF_VIDEO_FORMAT_R8_G8_B8 || + format == gxf::VideoFormat::GXF_VIDEO_FORMAT_B8_G8_R8 || + format == gxf::VideoFormat::GXF_VIDEO_FORMAT_R16_G16_B16 || + format == gxf::VideoFormat::GXF_VIDEO_FORMAT_B16_G16_R16 || + format == gxf::VideoFormat::GXF_VIDEO_FORMAT_R32_G32_B32 || + format == gxf::VideoFormat::GXF_VIDEO_FORMAT_B32_G32_R32 || + format == gxf::VideoFormat::GXF_VIDEO_FORMAT_NV12 || + format == gxf::VideoFormat::GXF_VIDEO_FORMAT_NV12_ER || + format == gxf::VideoFormat::GXF_VIDEO_FORMAT_NV24 || + format == gxf::VideoFormat::GXF_VIDEO_FORMAT_NV24_ER; +} + +template::type* = nullptr> +gxf::Expected AllocateUnpaddedVideoBuffer( + gxf::Handle frame, uint32_t width, uint32_t height, + gxf::MemoryStorageType storage_type, + gxf::Handle allocator) { + GXF_LOG_ERROR("Received unsupported video format!"); + return gxf::Unexpected{GXF_FAILURE}; +} + +template::type* = nullptr> +gxf::Expected AllocateUnpaddedVideoBuffer( + gxf::Handle frame, uint32_t width, uint32_t height, + gxf::MemoryStorageType storage_type, gxf::Handle allocator) { + if (width % 2 != 0 || height % 2 != 0) { + GXF_LOG_ERROR( + "Error: expected even width and height but received %u width and %u height", + width, height); + return gxf::Unexpected{GXF_FAILURE}; + } + NoPaddingColorPlanes nopadding_planes(width); + gxf::VideoFormatSize video_format_size; + auto size = video_format_size.size(width, height, nopadding_planes.planes); + std::vector color_planes{ + nopadding_planes.planes.begin(), nopadding_planes.planes.end()}; + gxf::VideoBufferInfo buffer_info{width, height, T, color_planes, + gxf::SurfaceLayout::GXF_SURFACE_LAYOUT_PITCH_LINEAR}; + return frame->resizeCustom(buffer_info, size, storage_type, allocator); +} + +template +gxf::Expected AllocateVideoBuffer( + gxf::Handle frame, uint32_t width, uint32_t height, + gxf::SurfaceLayout layout, gxf::MemoryStorageType storage_type, + gxf::Handle allocator) { + return frame->resize(width, height, layout, storage_type, allocator); +} + +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_image_proc/package.xml b/isaac_ros_image_proc/package.xml index 0ea271f..05d9eb3 100644 --- a/isaac_ros_image_proc/package.xml +++ b/isaac_ros_image_proc/package.xml @@ -21,7 +21,7 @@ SPDX-License-Identifier: Apache-2.0 isaac_ros_image_proc - 0.30.0 + 0.31.0 Single image rectification and color processing Hemal Shah diff --git a/isaac_ros_image_proc/src/image_flip_node.cpp b/isaac_ros_image_proc/src/image_flip_node.cpp index da8e2a2..cf4477d 100644 --- a/isaac_ros_image_proc/src/image_flip_node.cpp +++ b/isaac_ros_image_proc/src/image_flip_node.cpp @@ -35,7 +35,7 @@ constexpr char INPUT_COMPONENT_KEY[] = "image_flip/data_receiver"; constexpr char INPUT_DEFAULT_TENSOR_FORMAT[] = "nitros_image_rgb8"; constexpr char INPUT_TOPIC_NAME[] = "image"; -constexpr char OUTPUT_COMPONENT_KEY[] = "vault/vault"; +constexpr char OUTPUT_COMPONENT_KEY[] = "sink/sink"; constexpr char OUTPUT_DEFAULT_TENSOR_FORMAT[] = "nitros_image_rgb8"; constexpr char OUTPUT_TOPIC_NAME[] = "image_flipped"; diff --git a/isaac_ros_image_proc/src/image_format_converter_node.cpp b/isaac_ros_image_proc/src/image_format_converter_node.cpp index f1e2733..77a1d77 100644 --- a/isaac_ros_image_proc/src/image_format_converter_node.cpp +++ b/isaac_ros_image_proc/src/image_format_converter_node.cpp @@ -41,7 +41,7 @@ constexpr char INPUT_COMPONENT_KEY[] = "imageConverter/data_receiver"; constexpr char INPUT_DEFAULT_TENSOR_FORMAT[] = "nitros_image_rgb8"; constexpr char INPUT_TOPIC_NAME[] = "image_raw"; -constexpr char OUTPUT_COMPONENT_KEY[] = "vault/vault"; +constexpr char OUTPUT_COMPONENT_KEY[] = "sink/sink"; constexpr char OUTPUT_DEFAULT_TENSOR_FORMAT[] = "nitros_image_bgr8"; constexpr char OUTPUT_TOPIC_NAME[] = "image"; diff --git a/isaac_ros_image_proc/src/rectify_node.cpp b/isaac_ros_image_proc/src/rectify_node.cpp index 70f4c08..381aa37 100644 --- a/isaac_ros_image_proc/src/rectify_node.cpp +++ b/isaac_ros_image_proc/src/rectify_node.cpp @@ -46,11 +46,11 @@ constexpr char INPUT_COMPONENT_KEY[] = "input_compositor/image_in"; constexpr char INPUT_DEFAULT_TENSOR_FORMAT[] = "nitros_image_bgr8"; constexpr char INPUT_TOPIC_NAME[] = "image_raw"; -constexpr char OUTPUT_COMPONENT_KEY[] = "image_vault/vault"; +constexpr char OUTPUT_COMPONENT_KEY[] = "image_sink/sink"; constexpr char OUTPUT_DEFAULT_TENSOR_FORMAT[] = "nitros_image_bgr8"; constexpr char OUTPUT_TOPIC_NAME[] = "image_rect"; -constexpr char OUTPUT_CAM_COMPONENT_KEY[] = "camerainfo_vault/vault"; +constexpr char OUTPUT_CAM_COMPONENT_KEY[] = "camerainfo_sink/sink"; constexpr char OUTPUT_DEFAULT_CAM_INFO_FORMAT[] = "nitros_camera_info"; constexpr char OUTPUT_CAM_TOPIC_NAME[] = "camera_info_rect"; @@ -66,7 +66,8 @@ const std::vector> EXTENSIONS = { const std::vector PRESET_EXTENSION_SPEC_NAMES = { "isaac_ros_image_proc", }; -const std::vector EXTENSION_SPEC_FILENAMES = {}; +const std::vector EXTENSION_SPEC_FILENAMES = { +}; const std::vector GENERATOR_RULE_FILENAMES = { "config/namespace_injector_rule_rectify.yaml", }; @@ -141,14 +142,14 @@ void RectifyNode::preLoadGraphCallback() std::string w = "[" + std::to_string(output_width_) + "]"; NitrosNode::preLoadGraphSetParameter( "rectifier", - "nvidia::cvcore::tensor_ops::StreamUndistort", + "nvidia::isaac::tensor_ops::StreamUndistort", "regions_width", w); std::string h = "[" + std::to_string(output_height_) + "]"; NitrosNode::preLoadGraphSetParameter( "rectifier", - "nvidia::cvcore::tensor_ops::StreamUndistort", + "nvidia::isaac::tensor_ops::StreamUndistort", "regions_height", h); RCLCPP_DEBUG( @@ -164,9 +165,9 @@ void RectifyNode::postLoadGraphCallback() "[RectifyNode] postLoadGraphCallback()."); const gxf::optimizer::ComponentInfo component = { - "nvidia::gxf::Vault", // component_type_name - "vault", // component_name - "image_vault" // entity_name + "nvidia::isaac_ros::MessageRelay", // component_type_name + "sink", // component_name + "image_sink" // entity_name }; std::string image_format = getFinalDataFormat(component); uint64_t block_size = calculate_image_size(image_format, output_width_, output_height_); diff --git a/isaac_ros_image_proc/src/resize_node.cpp b/isaac_ros_image_proc/src/resize_node.cpp index d57f424..f68acf4 100644 --- a/isaac_ros_image_proc/src/resize_node.cpp +++ b/isaac_ros_image_proc/src/resize_node.cpp @@ -45,11 +45,11 @@ constexpr char INPUT_COMPONENT_KEY[] = "input_compositor/image_in"; constexpr char INPUT_DEFAULT_TENSOR_FORMAT[] = "nitros_image_bgr8"; constexpr char INPUT_TOPIC_NAME[] = "image"; -constexpr char OUTPUT_COMPONENT_KEY[] = "image_vault/vault"; +constexpr char OUTPUT_COMPONENT_KEY[] = "image_sink/sink"; constexpr char OUTPUT_DEFAULT_TENSOR_FORMAT[] = "nitros_image_bgr8"; constexpr char OUTPUT_TOPIC_NAME[] = "resize/image"; -constexpr char OUTPUT_CAM_COMPONENT_KEY[] = "camerainfo_vault/vault"; +constexpr char OUTPUT_CAM_COMPONENT_KEY[] = "camerainfo_sink/sink"; constexpr char OUTPUT_DEFAULT_CAM_INFO_FORMAT[] = "nitros_camera_info"; constexpr char OUTPUT_CAM_TOPIC_NAME[] = "resize/camera_info"; @@ -149,15 +149,15 @@ void ResizeNode::postLoadGraphCallback() // Update resize parameters getNitrosContext().setParameterUInt64( - "imageResizer", "nvidia::cvcore::tensor_ops::Resize", "output_width", + "imageResizer", "nvidia::isaac::tensor_ops::Resize", "output_width", (uint64_t)output_width_); getNitrosContext().setParameterUInt64( - "imageResizer", "nvidia::cvcore::tensor_ops::Resize", "output_height", + "imageResizer", "nvidia::isaac::tensor_ops::Resize", "output_height", (uint64_t)output_height_); getNitrosContext().setParameterBool( - "imageResizer", "nvidia::cvcore::tensor_ops::Resize", "keep_aspect_ratio", + "imageResizer", "nvidia::isaac::tensor_ops::Resize", "keep_aspect_ratio", keep_aspect_ratio_); // The minimum number of memory blocks is set based on the receiver queue capacity @@ -172,9 +172,9 @@ void ResizeNode::postLoadGraphCallback() output_width_, output_height_, keep_aspect_ratio_ ? "true" : "false"); const gxf::optimizer::ComponentInfo component = { - "nvidia::gxf::Vault", // component_type_name - "vault", // component_name - "image_vault" // entity_name + "nvidia::isaac_ros::MessageRelay", // component_type_name + "sink", // component_name + "image_sink" // entity_name }; std::string image_format = getFinalDataFormat(component); uint64_t block_size = calculate_image_size(image_format, output_width_, output_height_); diff --git a/isaac_ros_image_proc/test/isaac_ros_image_proc_test.py b/isaac_ros_image_proc/test/isaac_ros_image_proc_test.py index 688eccd..de065c3 100644 --- a/isaac_ros_image_proc/test/isaac_ros_image_proc_test.py +++ b/isaac_ros_image_proc/test/isaac_ros_image_proc_test.py @@ -94,7 +94,7 @@ def test_image_proc(self, test_folder) -> None: rclpy.spin_once(self.node, timeout_sec=0.1) # If we have received at least one message on each output topic, break - if all([len(received_messages.get(topic, [])) > 0 for topic in output_topics]): + if all(len(received_messages.get(topic, [])) > 0 for topic in output_topics): done = True break diff --git a/isaac_ros_image_proc/test/isaac_ros_rectify_oss_comparison_test.py b/isaac_ros_image_proc/test/isaac_ros_rectify_oss_comparison_test.py new file mode 100644 index 0000000..a45634f --- /dev/null +++ b/isaac_ros_image_proc/test/isaac_ros_rectify_oss_comparison_test.py @@ -0,0 +1,217 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +"""TimeStamp match test for the Isaac ROS Rectify node.""" + +import os +import pathlib +import time + +import cv2 +# Need to load sklearn first on Jetson. +from isaac_ros_test import IsaacROSBaseTest, JSONConversion +import launch_ros +import pytest +import rclpy +from sensor_msgs.msg import CameraInfo, Image + +VISUALIZE = False + + +@pytest.mark.rostest +def generate_test_description(): + """Generate launch description with all ROS 2 nodes for testing.""" + composable_nodes = [ + launch_ros.descriptions.ComposableNode( + name='left_rectify_node', + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::RectifyNode', + namespace=IsaacROSRectifyOSSTest.generate_namespace(), + parameters=[{ + 'output_width': 1920, + 'output_height': 1200 + }], + remappings=[ + ('image_raw', 'left/image_raw'), + ('camera_info', 'left/camerainfo'), + ('image_rect', 'left/image_rect'), + ('camera_info_rect', 'left/camera_info_rect') + ] + ), + launch_ros.descriptions.ComposableNode( + name='right_rectify_node', + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::RectifyNode', + namespace=IsaacROSRectifyOSSTest.generate_namespace(), + parameters=[{ + 'output_width': 1920, + 'output_height': 1200, + }], + remappings=[ + ('image_raw', 'right/image_raw'), + ('camera_info', 'right/camerainfo'), + ('image_rect', 'right/image_rect'), + ('camera_info_rect', 'right/camera_info_rect') + ] + ), + launch_ros.descriptions.ComposableNode( + name='left_oss_rectify_node', + package='image_proc', + plugin='image_proc::RectifyNode', + namespace=IsaacROSRectifyOSSTest.generate_namespace(), + parameters=[{ + 'width': 1920, + 'height': 1200, + 'use_scale': False + }], + remappings=[ + ('image', 'left/image_raw'), + ('camera_info', 'left/camerainfo'), + ('image_rect', 'left/image_rect_oss') + ] + ), + launch_ros.descriptions.ComposableNode( + name='right_oss_rectify_node', + package='image_proc', + plugin='image_proc::RectifyNode', + namespace=IsaacROSRectifyOSSTest.generate_namespace(), + parameters=[{ + 'width': 1920, + 'height': 1200, + 'use_scale': False + }], + remappings=[ + ('image', 'right/image_raw'), + ('camera_info', 'right/camerainfo'), + ('image_rect', 'right/image_rect_oss') + ] + )] + + rectify_container = launch_ros.actions.ComposableNodeContainer( + name='rectify_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=composable_nodes, + output='screen', + arguments=['--ros-args', '--log-level', 'info'], + ) + + return IsaacROSRectifyOSSTest.generate_test_description([rectify_container]) + + +class IsaacROSRectifyOSSTest(IsaacROSBaseTest): + """Validate that input timestamps match output timestamps.""" + + filepath = pathlib.Path(os.path.dirname(__file__)) + + @IsaacROSBaseTest.for_each_test_case(subfolder='rectify_stereo') + def test_rectify_chessboard(self, test_folder) -> None: + """Expect the output of OSS rectify and Isaac ROS rectify to be the similar.""" + self.generate_namespace_lookup(['left/image_raw', 'left/camerainfo', + 'right/image_raw', 'right/camerainfo', + 'left/image_rect', 'left/image_rect_oss', + 'right/image_rect', 'right/image_rect_oss']) + left_image_raw_pub = self.node.create_publisher( + Image, self.namespaces['left/image_raw'], self.DEFAULT_QOS) + left_camera_info_pub = self.node.create_publisher( + CameraInfo, self.namespaces['left/camerainfo'], self.DEFAULT_QOS) + right_image_raw_pub = self.node.create_publisher( + Image, self.namespaces['right/image_raw'], self.DEFAULT_QOS) + right_camera_info_pub = self.node.create_publisher( + CameraInfo, self.namespaces['right/camerainfo'], self.DEFAULT_QOS) + + received_messages = {} + left_image_rect_sub, right_image_rect_sub, \ + left_oss_image_rect_sub, right_oss_image_rect_sub = \ + self.create_logging_subscribers([ + ('left/image_rect', Image), + ('right/image_rect', Image), + ('left/image_rect_oss', Image), + ('right/image_rect_oss', Image), + ], received_messages, accept_multiple_messages=True) + try: + image_raw_left, _ = JSONConversion.load_chessboard_image_from_json( + test_folder / 'image_raw_left.json') + camera_info_left = JSONConversion.load_camera_info_from_json( + test_folder / 'camera_info_left.json') + image_raw_right, _ = JSONConversion.load_chessboard_image_from_json( + test_folder / 'image_raw_right.json') + camera_info_right = JSONConversion.load_camera_info_from_json( + test_folder / 'camera_info_right.json') + + # Wait at most TIMEOUT seconds for subscriber to respond + TIMEOUT = 5 + end_time = time.time() + TIMEOUT + + done = False + output_topics = ['left/image_rect', 'right/image_rect', + 'left/image_rect_oss', 'right/image_rect_oss'] + while time.time() < end_time: + # Synchronize timestamps on both messages + timestamp = self.node.get_clock().now().to_msg() + image_raw_left.header.stamp = timestamp + camera_info_left.header.stamp = timestamp + image_raw_right.header.stamp = timestamp + camera_info_right.header.stamp = timestamp + + # Publish test case + left_image_raw_pub.publish(image_raw_left) + left_camera_info_pub.publish(camera_info_left) + right_image_raw_pub.publish(image_raw_right) + right_camera_info_pub.publish(camera_info_right) + + rclpy.spin_once(self.node, timeout_sec=0.1) + # If we have received a message on the output topic, break + if all(len(received_messages.get(topic, [])) > 0 for topic in output_topics): + done = True + break + + self.assertTrue(done, + "Didn't receive output on each topic!\n" + 'Expected messages on:\n' + + '\n'.join(output_topics) + + '\nReceived messages on: \n' + + '\n'.join(received_messages.keys())) + image_left_rect = \ + self.bridge.imgmsg_to_cv2(received_messages['left/image_rect'][0]) + image_right_rect = \ + self.bridge.imgmsg_to_cv2(received_messages['right/image_rect'][0]) + image_left_rect_oss = \ + self.bridge.imgmsg_to_cv2(received_messages['left/image_rect_oss'][0]) + image_right_rect_oss = \ + self.bridge.imgmsg_to_cv2(received_messages['right/image_rect_oss'][0]) + if(VISUALIZE): + cv2.imwrite('image_left_rect.png', image_left_rect) + cv2.imwrite('image_right_rect.png', image_right_rect) + cv2.imwrite('image_left_rect_oss.png', image_left_rect_oss) + cv2.imwrite('image_right_rect_oss.png', image_right_rect_oss) + cv2.imwrite('image_left_diff.png', + cv2.absdiff(image_left_rect, image_left_rect_oss)) + cv2.imwrite('image_right_diff.png', + cv2.absdiff(image_right_rect, image_right_rect_oss)) + self.assertImagesEqual(image_left_rect, image_left_rect_oss, 0.00001) + self.assertImagesEqual(image_right_rect, image_right_rect_oss, 0.00001) + finally: + self.assertTrue(self.node.destroy_subscription(left_image_rect_sub)) + self.assertTrue(self.node.destroy_subscription(right_image_rect_sub)) + self.assertTrue(self.node.destroy_subscription(left_oss_image_rect_sub)) + self.assertTrue(self.node.destroy_subscription(right_oss_image_rect_sub)) + self.assertTrue(self.node.destroy_publisher(left_image_raw_pub)) + self.assertTrue(self.node.destroy_publisher(left_camera_info_pub)) + self.assertTrue(self.node.destroy_publisher(right_image_raw_pub)) + self.assertTrue(self.node.destroy_publisher(right_camera_info_pub)) diff --git a/isaac_ros_image_proc/test/isaac_ros_rectify_stereo_epipolar_test.py b/isaac_ros_image_proc/test/isaac_ros_rectify_stereo_epipolar_test.py new file mode 100644 index 0000000..2d5d127 --- /dev/null +++ b/isaac_ros_image_proc/test/isaac_ros_rectify_stereo_epipolar_test.py @@ -0,0 +1,222 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Epipolar line validation test for the Isaac ROS Rectify node.""" + +import os +import pathlib +import random +import time + +import cv2 # noqa: I100 +from isaac_ros_test import IsaacROSBaseTest, JSONConversion +import launch_ros +import pytest +import rclpy +from sensor_msgs.msg import CameraInfo, Image + +VISUALIZE = False + + +@pytest.mark.rostest +def generate_test_description(): + """Generate launch description with all ROS 2 nodes for testing.""" + composable_nodes = [ + launch_ros.descriptions.ComposableNode( + name='left_rectify_node', + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::RectifyNode', + namespace=IsaacROSStereoRectifyEpipolarTest.generate_namespace(), + parameters=[{ + 'output_width': 1920, + 'output_height': 1200 + }], + remappings=[ + ('image_raw', 'left_image_raw'), + ('camera_info', 'left_camera_info'), + ('image_rect', 'left_image_rect') + ] + ), + launch_ros.descriptions.ComposableNode( + name='right_rectify_node', + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::RectifyNode', + namespace=IsaacROSStereoRectifyEpipolarTest.generate_namespace(), + parameters=[{ + 'output_width': 1920, + 'output_height': 1200, + }], + remappings=[ + ('image_raw', 'right_image_raw'), + ('camera_info', 'right_camera_info'), + ('image_rect', 'right_image_rect') + ] + )] + + rectify_container = launch_ros.actions.ComposableNodeContainer( + name='rectify_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=composable_nodes, + output='screen', + arguments=['--ros-args', '--log-level', 'info'], + ) + + return IsaacROSStereoRectifyEpipolarTest.generate_test_description([rectify_container]) + + +class IsaacROSStereoRectifyEpipolarTest(IsaacROSBaseTest): + """Validate stereo image rectification with chessboard images.""" + + filepath = pathlib.Path(os.path.dirname(__file__)) + + @IsaacROSBaseTest.for_each_test_case(subfolder='rectify_stereo') + def test_rectify_chessboard(self, test_folder) -> None: + """Expect the node to rectify chessboard images.""" + self.generate_namespace_lookup(['left_image_raw', 'left_camera_info', + 'right_image_raw', 'right_camera_info']) + left_image_raw_pub = self.node.create_publisher( + Image, self.namespaces['left_image_raw'], self.DEFAULT_QOS) + left_camera_info_pub = self.node.create_publisher( + CameraInfo, self.namespaces['left_camera_info'], self.DEFAULT_QOS) + right_image_raw_pub = self.node.create_publisher( + Image, self.namespaces['right_image_raw'], self.DEFAULT_QOS) + right_camera_info_pub = self.node.create_publisher( + CameraInfo, self.namespaces['right_camera_info'], self.DEFAULT_QOS) + + received_messages = [] + self.create_exact_time_sync_logging_subscribers( + [('left_image_rect', Image), ('right_image_rect', Image)], received_messages, + accept_multiple_messages=True) + + image_raw_left, left_chessboard_dims = JSONConversion.load_chessboard_image_from_json( + test_folder / 'image_raw_left.json') + camera_info_left = JSONConversion.load_camera_info_from_json( + test_folder / 'camera_info_left.json') + image_raw_right, right_chessboard_dims = JSONConversion.load_chessboard_image_from_json( + test_folder / 'image_raw_right.json') + camera_info_right = JSONConversion.load_camera_info_from_json( + test_folder / 'camera_info_right.json') + + # Wait at most TIMEOUT seconds for subscriber to respond + TIMEOUT = 2 + end_time = time.time() + TIMEOUT + + done = False + while time.time() < end_time: + # Synchronize timestamps on both messages + timestamp = self.node.get_clock().now().to_msg() + image_raw_left.header.stamp = timestamp + camera_info_left.header.stamp = timestamp + image_raw_right.header.stamp = timestamp + camera_info_right.header.stamp = timestamp + + # Publish test case + left_image_raw_pub.publish(image_raw_left) + left_camera_info_pub.publish(camera_info_left) + right_image_raw_pub.publish(image_raw_right) + right_camera_info_pub.publish(camera_info_right) + + rclpy.spin_once(self.node, timeout_sec=0.1) + + # If we have received a message on the output topic, break + if len(received_messages) > 0: + done = True + break + + self.assertTrue(done, "Didn't receive output on image_rect topics!") + + for received_message in received_messages: + # Collect received image + left_image_rect = self.bridge.imgmsg_to_cv2(received_message[0]) + right_image_rect = self.bridge.imgmsg_to_cv2(received_message[1]) + # Convert to grayscale and find chessboard corners in rectified image + left_image_rect_gray = cv2.cvtColor(left_image_rect, cv2.COLOR_BGR2GRAY) + left_ret, left_corners = cv2.findChessboardCorners(left_image_rect_gray, + left_chessboard_dims, None) + self.assertTrue(left_ret, "Couldn't find chessboard corners in output left image!") + right_image_rect_gray = cv2.cvtColor(right_image_rect, cv2.COLOR_BGR2GRAY) + right_ret, right_corners = cv2.findChessboardCorners(right_image_rect_gray, + right_chessboard_dims, None) + self.assertTrue(right_ret, "Couldn't find chessboard corners in output right image!") + # Extract the x and y coordinates of the corners in left_corners and right_corners + x_coords_left = [c[0][0] for c in left_corners] + y_coords_left = [c[0][1] for c in left_corners] + x_coords_right = [c[0][0] for c in right_corners] + y_coords_right = [c[0][1] for c in right_corners] + if(VISUALIZE): + # Draw lines of the same color at the avergae row value for all corners + # in the left and right image + cv2.drawChessboardCorners(left_image_rect, left_chessboard_dims, + left_corners, left_ret) + cv2.drawChessboardCorners(right_image_rect, right_chessboard_dims, + right_corners, right_ret) + # Draw randomly colored lines connecting the corresponding corners + for i in range(min(len(left_corners), len(right_corners))): + average_y = (y_coords_left[i]+y_coords_right[i])/2 + pt1 = (0, int(average_y)) + pt2 = (left_image_rect.shape[1], int(average_y)) + random_color = (random.randint(0, 255), + random.randint(0, 255), + random.randint(0, 255)) + cv2.line(left_image_rect, pt1, pt2, random_color, 1) + cv2.line(right_image_rect, pt1, pt2, random_color, 1) + cv2.imwrite('left_image_rect.png', left_image_rect) + cv2.imwrite('right_image_rect.png', right_image_rect) + + """ + Test 1: + Check if the row values of the same corners of the chessboard + in the left and right images are within threshold + """ + # Compute the differences in row values + row_diffs = [abs(y_coords_left[i] - y_coords_right[i]) + for i in range(min(len(left_corners), len(right_corners)))] + # Allows test pass if same features are within of 4 pixels in both images + CORNER_ROW_DIFF_THRESHOLD = 4 + if(VISUALIZE): + print('CORNER_ROW_DIFF_THRESHOLD :') + print(CORNER_ROW_DIFF_THRESHOLD) + print('row_diffs :') + print(row_diffs) + self.assertFalse( + any(diff > CORNER_ROW_DIFF_THRESHOLD for diff in row_diffs), + 'Difference between corners row values in left and right images' + 'are not within threshold') + + """ + Test 2: + Check if the slopes of the lines connecting each corresponding corners in + the left and right images are within threshold from the mean. + This test checks if these lines(epipolar lines) are parallel + """ + # Compute the slopes of lines between corresponding corners + slopes = [(y_coords_right[i]-y_coords_left[i])/(x_coords_right[i]-x_coords_left[i]) + for i in range(min(len(left_corners), len(right_corners)))] + # Create a list of difference betwen the slope and average slope + mean_slope_diffs = slopes-(sum(slopes) / len(slopes)) + EPIPOLAR_LINES_SLOPE_DIFF_THRESHOLD = 0.005 + if(VISUALIZE): + print('EPIPOLAR_LINES_SLOPE_DIFF_THRESHOLD :') + print(EPIPOLAR_LINES_SLOPE_DIFF_THRESHOLD) + print('mean_slope_diffs :') + print(mean_slope_diffs) + self.assertFalse( + any(mean_slope_diff > EPIPOLAR_LINES_SLOPE_DIFF_THRESHOLD + for mean_slope_diff in mean_slope_diffs), + 'Epipolar lines are not parallel!') diff --git a/isaac_ros_image_proc/test/isaac_ros_rectify_timestamp_match_test.py b/isaac_ros_image_proc/test/isaac_ros_rectify_timestamp_match_test.py new file mode 100644 index 0000000..4196e48 --- /dev/null +++ b/isaac_ros_image_proc/test/isaac_ros_rectify_timestamp_match_test.py @@ -0,0 +1,131 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +"""TimeStamp match test for the Isaac ROS Rectify node.""" + +import os +import pathlib +import time + +# Need to load sklearn first on Jetson. +from isaac_ros_test import IsaacROSBaseTest, JSONConversion +import launch_ros +import pytest +import rclpy +from sensor_msgs.msg import CameraInfo, Image + + +@pytest.mark.rostest +def generate_test_description(): + """Generate launch description with all ROS 2 nodes for testing.""" + composable_nodes = [ + launch_ros.descriptions.ComposableNode( + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::RectifyNode', + name='rectify_node', + namespace=IsaacROSRectifyTest.generate_namespace(), + remappings=[('image', 'image_raw')], + parameters=[{ + 'output_width': 1280, + 'output_height': 800, + }] + )] + + rectify_container = launch_ros.actions.ComposableNodeContainer( + name='rectify_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=composable_nodes, + output='screen', + arguments=['--ros-args', '--log-level', 'info'], + ) + + return IsaacROSRectifyTest.generate_test_description([rectify_container]) + + +class IsaacROSRectifyTest(IsaacROSBaseTest): + """Validate that input timestamps match output timestamps.""" + + filepath = pathlib.Path(os.path.dirname(__file__)) + + @IsaacROSBaseTest.for_each_test_case(subfolder='rectify') + def test_rectify_chessboard(self, test_folder) -> None: + """ + Expect the node to accept matched time stamps(image and camera_info). + + And generate matched time stamps outputs(image and camera_info). + """ + self.generate_namespace_lookup(['image_raw', 'camera_info', + 'image_rect', 'camera_info_rect']) + + image_raw_pub = self.node.create_publisher( + Image, self.namespaces['image_raw'], self.DEFAULT_QOS) + camera_info_pub = self.node.create_publisher( + CameraInfo, self.namespaces['camera_info'], self.DEFAULT_QOS) + + received_messages = [] + self.create_exact_time_sync_logging_subscribers( + [('image_rect', Image), ('camera_info_rect', CameraInfo)], + received_messages, accept_multiple_messages=True) + + try: + image_raw = JSONConversion.load_image_from_json( + test_folder / 'image_raw.json') + camera_info = JSONConversion.load_camera_info_from_json( + test_folder / 'camera_info.json') + + # Wait at most TIMEOUT seconds for subscriber to respond + TIMEOUT = 10 + end_time = time.time() + TIMEOUT + + done = False + + input_timestamps = [] + while time.time() < end_time: + # Synchronize timestamps on both input messages + timestamp = self.node.get_clock().now().to_msg() + image_raw.header.stamp = timestamp + camera_info.header.stamp = timestamp + input_timestamps.append(timestamp) + + # Publish test case over both topics + image_raw_pub.publish(image_raw) + camera_info_pub.publish(camera_info) + + rclpy.spin_once(self.node, timeout_sec=0.5) + + # If we have received at least one synchronized message output, break + if len(received_messages) > 0: + done = True + break + + self.assertTrue(done, "Didn't receive time synced output!") + for received_message in received_messages: + self.assertTrue(received_message[0].header.stamp == + received_message[1].header.stamp, + 'Timestamps are not synced!') + self.assertTrue(received_message[0].header.stamp in input_timestamps, + 'Rectified image timestamp not in list of input timestamps!') + self.assertTrue(received_message[1].header.stamp in input_timestamps, + 'Rectified camera info timestamp not in list of \ + input timestamps!') + self.assertTrue(done, "Didn't receive time synced output!") + + finally: + self.assertTrue(self.node.destroy_publisher(image_raw_pub)) + self.assertTrue(self.node.destroy_publisher(camera_info_pub)) diff --git a/isaac_ros_image_proc/test/isaac_ros_resize_invalid_test.py b/isaac_ros_image_proc/test/isaac_ros_resize_invalid_test.py index 169aa46..2272b06 100644 --- a/isaac_ros_image_proc/test/isaac_ros_resize_invalid_test.py +++ b/isaac_ros_image_proc/test/isaac_ros_resize_invalid_test.py @@ -111,9 +111,9 @@ def test_resize_invalid(self, test_folder) -> None: self.assertIn('/rosout', received_messages, "Didn't receive output on /rosout topic!") # Make sure that at least one output log message is a non-empty error - self.assertTrue(any([ + self.assertTrue(any( LoggingSeverity(rosout.level) == LoggingSeverity.ERROR and len(rosout.msg) > 0 - for rosout in received_messages['/rosout']]), + for rosout in received_messages['/rosout']), 'No message with non-empty message and Error severity!') # Make sure no output image was received in the error case diff --git a/isaac_ros_image_proc/test/test_cases/generate_test_cases.py b/isaac_ros_image_proc/test/test_cases/generate_test_cases.py index 14fa90b..0bc5fd8 100644 --- a/isaac_ros_image_proc/test/test_cases/generate_test_cases.py +++ b/isaac_ros_image_proc/test/test_cases/generate_test_cases.py @@ -204,7 +204,7 @@ def generate_ground_truth( rclpy.spin_once(self.node, timeout_sec=TIMEOUT) # If we have received exactly one message on each output topic, break - if all([topic in received_images for topic in output_topics]): + if all(topic in received_images for topic in output_topics): done = True break assert done, "Didn't receive output messages on all subscribers! " \ diff --git a/isaac_ros_image_proc/test/test_cases/rectify/chessboard0/camera_info.json b/isaac_ros_image_proc/test/test_cases/rectify/chessboard0/camera_info.json index 4e5e5b7..f2b27e6 100644 --- a/isaac_ros_image_proc/test/test_cases/rectify/chessboard0/camera_info.json +++ b/isaac_ros_image_proc/test/test_cases/rectify/chessboard0/camera_info.json @@ -35,13 +35,13 @@ 1.0 ], "P": [ - 1.0, - 0.0, - 0.0, + 434.943999, 0.0, + 651.073921, 0.0, - 1.0, 0.0, + 431.741273, + 441.878037, 0.0, 0.0, 0.0, diff --git a/isaac_ros_image_proc/test/test_cases/rectify/chessboard1/camera_info.json b/isaac_ros_image_proc/test/test_cases/rectify/chessboard1/camera_info.json index 4e5e5b7..f2b27e6 100644 --- a/isaac_ros_image_proc/test/test_cases/rectify/chessboard1/camera_info.json +++ b/isaac_ros_image_proc/test/test_cases/rectify/chessboard1/camera_info.json @@ -35,13 +35,13 @@ 1.0 ], "P": [ - 1.0, - 0.0, - 0.0, + 434.943999, 0.0, + 651.073921, 0.0, - 1.0, 0.0, + 431.741273, + 441.878037, 0.0, 0.0, 0.0, diff --git a/isaac_ros_image_proc/test/test_cases/rectify_stereo/stereo_chessboard0/camera_info_left.json b/isaac_ros_image_proc/test/test_cases/rectify_stereo/stereo_chessboard0/camera_info_left.json new file mode 100644 index 0000000..50600ff --- /dev/null +++ b/isaac_ros_image_proc/test/test_cases/rectify_stereo/stereo_chessboard0/camera_info_left.json @@ -0,0 +1,54 @@ +{ + "header": { + "frame_id": "left_cam" + }, + "width": 1920, + "height": 1200, + "distortion_model": "rational_polynomial", + "D": [ + 0.25965502858161926, + 0.20133642852306366, + -0.0001395938015775755, + -5.49034884897992e-05, + 0.03186598792672157, + 0.6291006207466125, + 0.21190467476844788, + 0.10783819109201431 + ], + "K": [ + 962.4163818359375, + 0.0, + 954.67724609375, + 0.0, + 960.2732543945312, + 593.6239013671875, + 0.0, + 0.0, + 1.0 + ], + "R": [ + 0.9999997615814209, + -0.0006003116723150015, + -0.00025449667009525, + 0.000600108818616718, + 0.9999995231628418, + -0.0007963633397594094, + 0.0002549746131990105, + 0.0007962104282341897, + 0.9999996423721313 + ], + "P": [ + 831.0263061523438, + 0.0, + 935.90673828125, + 0.0, + 0.0, + 831.0263061523438, + 579.66748046875, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0 + ] +} \ No newline at end of file diff --git a/isaac_ros_image_proc/test/test_cases/rectify_stereo/stereo_chessboard0/camera_info_right.json b/isaac_ros_image_proc/test/test_cases/rectify_stereo/stereo_chessboard0/camera_info_right.json new file mode 100644 index 0000000..bad4f48 --- /dev/null +++ b/isaac_ros_image_proc/test/test_cases/rectify_stereo/stereo_chessboard0/camera_info_right.json @@ -0,0 +1,54 @@ +{ + "header": { + "frame_id": "left_cam" + }, + "width": 1920, + "height": 1200, + "distortion_model": "rational_polynomial", + "D": [ + 10.817594528198242, + 8.044410705566406, + -0.00013254497025627643, + -7.826957880752161e-05, + 0.5253813862800598, + 11.172098159790039, + 12.012029647827148, + 2.4224164485931396 + ], + "K": [ + 958.9111328125, + 0.0, + 951.4505615234375, + 0.0, + 956.7769775390625, + 594.925537109375, + 0.0, + 0.0, + 1.0 + ], + "R": [ + 0.9999944567680359, + -0.00031326754833571613, + -0.003315137466415763, + 0.0003159072657581419, + 0.9999996423721313, + 0.0007957643829286098, + 0.0033148869406431913, + -0.0007968072313815355, + 0.999994158744812 + ], + "P": [ + 831.0263061523438, + 0.0, + 935.90673828125, + -124.35779290307437, + 0.0, + 831.0263061523438, + 579.66748046875, + 4.6878754801582545e-05, + 0.0, + 0.0, + 1.0, + 0.0004960919613949955 + ] +} \ No newline at end of file diff --git a/isaac_ros_image_proc/test/test_cases/rectify_stereo/stereo_chessboard0/image_raw_left.json b/isaac_ros_image_proc/test/test_cases/rectify_stereo/stereo_chessboard0/image_raw_left.json new file mode 100644 index 0000000..7a96083 --- /dev/null +++ b/isaac_ros_image_proc/test/test_cases/rectify_stereo/stereo_chessboard0/image_raw_left.json @@ -0,0 +1,8 @@ +{ + "image": "image_raw_left.png", + "encoding": "bgr8", + "chessboard": { + "width": 9, + "height": 7 + } +} \ No newline at end of file diff --git a/isaac_ros_image_proc/test/test_cases/rectify_stereo/stereo_chessboard0/image_raw_left.png b/isaac_ros_image_proc/test/test_cases/rectify_stereo/stereo_chessboard0/image_raw_left.png new file mode 100644 index 0000000..6dc29c3 --- /dev/null +++ b/isaac_ros_image_proc/test/test_cases/rectify_stereo/stereo_chessboard0/image_raw_left.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4290e828b8d965bdcd18ff5d3a2be18343dbeccb4d3904f12f1bf523de2afae0 +size 3373324 diff --git a/isaac_ros_image_proc/test/test_cases/rectify_stereo/stereo_chessboard0/image_raw_right.json b/isaac_ros_image_proc/test/test_cases/rectify_stereo/stereo_chessboard0/image_raw_right.json new file mode 100644 index 0000000..666039c --- /dev/null +++ b/isaac_ros_image_proc/test/test_cases/rectify_stereo/stereo_chessboard0/image_raw_right.json @@ -0,0 +1,8 @@ +{ + "image": "image_raw_right.png", + "encoding": "bgr8", + "chessboard": { + "width": 9, + "height": 7 + } +} \ No newline at end of file diff --git a/isaac_ros_image_proc/test/test_cases/rectify_stereo/stereo_chessboard0/image_raw_right.png b/isaac_ros_image_proc/test/test_cases/rectify_stereo/stereo_chessboard0/image_raw_right.png new file mode 100644 index 0000000..b4f53ee --- /dev/null +++ b/isaac_ros_image_proc/test/test_cases/rectify_stereo/stereo_chessboard0/image_raw_right.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:dbd50c945dc6d441204853b69218102ea60c23f59c25b2d31a93db74cf03169c +size 3631306 diff --git a/isaac_ros_stereo_image_proc/config/nitros_disparity_node.yaml b/isaac_ros_stereo_image_proc/config/nitros_disparity_node.yaml index ec50a2d..0f9d211 100644 --- a/isaac_ros_stereo_image_proc/config/nitros_disparity_node.yaml +++ b/isaac_ros_stereo_image_proc/config/nitros_disparity_node.yaml @@ -1,6 +1,6 @@ %YAML 1.2 # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# Copyright (c) 2022-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -161,7 +161,7 @@ components: - name: disparity_in type: nvidia::gxf::DoubleBufferReceiver parameters: - capacity: 1 + capacity: 2 policy: 0 - type: nvidia::gxf::MessageAvailableSchedulingTerm parameters: @@ -170,7 +170,7 @@ components: - name: left_cam_receiver type: nvidia::gxf::DoubleBufferReceiver parameters: - capacity: 1 + capacity: 2 policy: 0 - type: nvidia::gxf::MessageAvailableSchedulingTerm parameters: @@ -179,7 +179,7 @@ components: - name: right_cam_receiver type: nvidia::gxf::DoubleBufferReceiver parameters: - capacity: 1 + capacity: 2 policy: 0 - type: nvidia::gxf::MessageAvailableSchedulingTerm parameters: @@ -203,7 +203,7 @@ components: min_disparity: 0 max_disparity: 64 --- -name: vault +name: sink components: - name: signal type: nvidia::gxf::DoubleBufferReceiver @@ -214,12 +214,10 @@ components: parameters: receiver: signal min_size: 1 -- name: vault - type: nvidia::gxf::Vault +- name: sink + type: nvidia::isaac_ros::MessageRelay parameters: source: signal - max_waiting_count: 1 - drop_waiting: false --- name: connections components: @@ -246,12 +244,17 @@ components: - type: nvidia::gxf::Connection parameters: source: egress/data_transmitter - target: vault/signal + target: sink/signal --- components: -- type: nvidia::gxf::GreedyScheduler +- name: clock + type: nvidia::gxf::RealtimeClock +- type: nvidia::gxf::MultiThreadScheduler parameters: clock: clock stop_on_deadlock: false -- name: clock - type: nvidia::gxf::RealtimeClock \ No newline at end of file + check_recession_period_ms: 1 + worker_thread_number: 2 +- type: nvidia::gxf::JobStatistics + parameters: + clock: clock \ No newline at end of file diff --git a/isaac_ros_stereo_image_proc/config/nitros_point_cloud_node.yaml b/isaac_ros_stereo_image_proc/config/nitros_point_cloud_node.yaml index 2b5e4d2..7b1024f 100644 --- a/isaac_ros_stereo_image_proc/config/nitros_point_cloud_node.yaml +++ b/isaac_ros_stereo_image_proc/config/nitros_point_cloud_node.yaml @@ -1,6 +1,6 @@ %YAML 1.2 # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# Copyright (c) 2022-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -172,23 +172,21 @@ components: use_color: false unit_scaling: 1 --- -name: vault +name: sink components: - name: signal type: nvidia::gxf::DoubleBufferReceiver parameters: - capacity: 1 + capacity: 2 policy: 0 - type: nvidia::gxf::MessageAvailableSchedulingTerm parameters: receiver: signal min_size: 1 -- name: vault - type: nvidia::gxf::Vault +- name: sink + type: nvidia::isaac_ros::MessageRelay parameters: source: signal - max_waiting_count: 1 - drop_waiting: false --- name: connections components: @@ -211,12 +209,17 @@ components: - type: nvidia::gxf::Connection parameters: source: point_cloud/data_transmitter - target: vault/signal + target: sink/signal --- components: -- type: nvidia::gxf::GreedyScheduler +- name: clock + type: nvidia::gxf::RealtimeClock +- type: nvidia::gxf::MultiThreadScheduler parameters: clock: clock stop_on_deadlock: false -- name: clock - type: nvidia::gxf::RealtimeClock \ No newline at end of file + check_recession_period_ms: 1 + worker_thread_number: 2 +- type: nvidia::gxf::JobStatistics + parameters: + clock: clock \ No newline at end of file diff --git a/isaac_ros_stereo_image_proc/package.xml b/isaac_ros_stereo_image_proc/package.xml index cabd5a3..40fd197 100644 --- a/isaac_ros_stereo_image_proc/package.xml +++ b/isaac_ros_stereo_image_proc/package.xml @@ -21,7 +21,7 @@ SPDX-License-Identifier: Apache-2.0 isaac_ros_stereo_image_proc - 0.30.0 + 0.31.0 Stereo and disparity processing Hemal Shah diff --git a/isaac_ros_stereo_image_proc/src/disparity_node.cpp b/isaac_ros_stereo_image_proc/src/disparity_node.cpp index 1f0806e..340ca9b 100644 --- a/isaac_ros_stereo_image_proc/src/disparity_node.cpp +++ b/isaac_ros_stereo_image_proc/src/disparity_node.cpp @@ -52,7 +52,7 @@ constexpr char INPUT_LEFT_CAMERA_TOPIC_NAME[] = "left/camera_info"; constexpr char INPUT_RIGHT_CAM_COMPONENT_KEY[] = "sync/right_cam_receiver"; constexpr char INPUT_RIGHT_CAMERA_TOPIC_NAME[] = "right/camera_info"; -constexpr char OUTPUT_COMPONENT_KEY[] = "vault/vault"; +constexpr char OUTPUT_COMPONENT_KEY[] = "sink/sink"; constexpr char OUTPUT_DEFAULT_TENSOR_FORMAT[] = "nitros_disparity_image_32FC1"; constexpr char OUTPUT_TOPIC_NAME[] = "disparity"; diff --git a/isaac_ros_stereo_image_proc/src/point_cloud_node.cpp b/isaac_ros_stereo_image_proc/src/point_cloud_node.cpp index 71f1a3f..6c3c334 100644 --- a/isaac_ros_stereo_image_proc/src/point_cloud_node.cpp +++ b/isaac_ros_stereo_image_proc/src/point_cloud_node.cpp @@ -54,7 +54,7 @@ constexpr char INPUT_LEFT_CAMERA_TOPIC_NAME[] = "left/camera_info"; constexpr char INPUT_RIGHT_CAM_COMPONENT_KEY[] = "sync/right_cam_receiver"; constexpr char INPUT_RIGHT_CAMERA_TOPIC_NAME[] = "right/camera_info"; -constexpr char OUTPUT_COMPONENT_KEY[] = "vault/vault"; +constexpr char OUTPUT_COMPONENT_KEY[] = "sink/sink"; constexpr char OUTPUT_DEFAULT_TENSOR_FORMAT[] = "nitros_point_cloud"; constexpr char OUTPUT_TOPIC_NAME[] = "points2"; diff --git a/isaac_ros_stereo_image_proc/test/isaac_ros_stereo_pipeline_output_compare.py b/isaac_ros_stereo_image_proc/test/isaac_ros_stereo_pipeline_output_compare.py index 15b068f..ef65ce8 100644 --- a/isaac_ros_stereo_image_proc/test/isaac_ros_stereo_pipeline_output_compare.py +++ b/isaac_ros_stereo_image_proc/test/isaac_ros_stereo_pipeline_output_compare.py @@ -148,7 +148,7 @@ def test_comparison(self, test_folder): rclpy.spin_once(self.node, timeout_sec=(0.1)) - if all([len(messages) >= 1 for messages in received_messages.values()]): + if all(len(messages) >= 1 for messages in received_messages.values()): done = True break diff --git a/isaac_ros_stereo_image_proc/test/isaac_ros_stereo_point_cloud_OSS_test.py b/isaac_ros_stereo_image_proc/test/isaac_ros_stereo_point_cloud_OSS_test.py index 18f607b..90592d8 100644 --- a/isaac_ros_stereo_image_proc/test/isaac_ros_stereo_point_cloud_OSS_test.py +++ b/isaac_ros_stereo_image_proc/test/isaac_ros_stereo_point_cloud_OSS_test.py @@ -136,10 +136,10 @@ def test_comparison(self, test_folder): rclpy.spin_once(self.node, timeout_sec=(0.1)) - if all([ + if all( len(messages) >= 1 for messages in received_messages.values() - ]): + ): done = True break self.assertTrue(done) diff --git a/isaac_ros_stereo_image_proc/test/isaac_ros_stereo_point_cloud_output_compare.py b/isaac_ros_stereo_image_proc/test/isaac_ros_stereo_point_cloud_output_compare.py index 2499d9a..ff8cecc 100644 --- a/isaac_ros_stereo_image_proc/test/isaac_ros_stereo_point_cloud_output_compare.py +++ b/isaac_ros_stereo_image_proc/test/isaac_ros_stereo_point_cloud_output_compare.py @@ -139,10 +139,10 @@ def test_comparison(self, test_folder): rclpy.spin_once(self.node, timeout_sec=(0.1)) - if all([ + if all( len(messages) >= 1 for messages in received_messages.values() - ]): + ): done = True break self.assertTrue(done)