diff --git a/.gitattributes b/.gitattributes index 9af4bce..c6ed24f 100644 --- a/.gitattributes +++ b/.gitattributes @@ -22,6 +22,7 @@ # ROS Bags **/resources/**/*.db3 filter=lfs diff=lfs merge=lfs -text **/resources/**/*.yaml filter=lfs diff=lfs merge=lfs -text +**/resources/**/*.bag filter=lfs diff=lfs merge=lfs -text # DNN Model files *.onnx filter=lfs diff=lfs merge=lfs -text \ No newline at end of file diff --git a/README.md b/README.md index 84777d0..89ec8db 100644 --- a/README.md +++ b/README.md @@ -1,378 +1,96 @@ # Isaac ROS Pose Estimation -
+Deep learned, hardware-accelerated 3D object pose estimation -## Overview - -Isaac ROS Pose Estimation contains ROS 2 packages to predict the pose of an object. `isaac_ros_dope` provides a pose estimation method using 3D bounding cuboid dimensions of a known object in an input image. `isaac_ros_centerpose` provides a pose estimation method using 3D bounding cuboid dimensions of unknown object instances in a known category of objects from an input image. `isaac_ros_dope` and `isaac_ros_centerpose` use GPU acceleration for DNN inference to estimate the pose of an object. The output prediction can be used by perception functions when fusing with a corresponding depth to provide the 3D pose of an object and distance for navigation or manipulation. - -
- -`isaac_ros_dope` is used in a graph of nodes to estimate the pose of a known object with 3D bounding cuboid dimensions. To produce the estimate, a [DOPE](https://github.com/NVlabs/Deep_Object_Pose) (Deep Object Pose Estimation) pre-trained model is required. Input images may need to be cropped and resized to maintain the aspect ratio and match the input resolution of DOPE. After DOPE has produced an estimate, the DNN decoder will use the specified object type to transform using belief maps to output object poses. +
image
-NVLabs has provided a DOPE pre-trained model using the [HOPE](https://github.com/swtyree/hope-dataset) dataset. HOPE stands for household objects for pose estimation and is a research-oriented dataset using toy grocery objects and 3D textured meshes of the objects for training on synthetic data. To use DOPE for other objects that are relevant to your application, it needs to be trained with another dataset targeting these objects. For example, DOPE has been trained to detect dollies for use with a mobile robot that navigates under, lifts, and moves that type of dolly. - -`isaac_ros_centerpose` has similarities to `isaac_ros_dope` in that both estimate an object pose; however, `isaac_ros_centerpose` provides additional functionality. The [CenterPose](https://github.com/NVlabs/CenterPose) DNN performs object detection on the image, generates 2D keypoints for the object, estimates the 6-DoF pose, and regresses relative 3D bounding cuboid dimensions. This is performed on a known object class without knowing the instance--for example, detecting a chair without having trained on images of all chairs. NVLabs has provided pre-trained models for the CenterPose model; however, as with the DOPE model, it needs to be trained with another dataset targeting objects that are specific to your application. - -Pose estimation is a compute-intensive task and not performed at the frame rate of an input camera. To make efficient use of resources, object pose is estimated for a single frame and used as an input to navigation. Additional object pose estimates are computed to further refine navigation in progress at a lower frequency than the input rate of a typical camera. +## Overview -Packages in this repository rely on accelerated DNN model inference using [Triton](https://github.com/triton-inference-server/server) or [TensorRT](https://developer.nvidia.com/tensorrt) from [Isaac ROS DNN Inference](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_dnn_inference). +[Isaac ROS Pose Estimation](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_pose_estimation) +contains ROS 2 packages to predict the pose of an object. +`isaac_ros_dope` estimates the object’s pose using the 3D +bounding cuboid dimensions of a known object in an input image. +`isaac_ros_centerpose` estimates the object’s pose using the 3D +bounding cuboid dimensions of unknown object instances in a known +category of objects from an input image. `isaac_ros_dope` and +`isaac_ros_centerpose` use GPU acceleration for DNN inference to +estimate the pose of an object. The output prediction can be used by +perception functions when fusing with the corresponding depth to provide +the 3D pose of an object and distance for navigation or manipulation. + +
image
+ +`isaac_ros_dope` is used in a graph of nodes to estimate the pose of a +known object with 3D bounding cuboid dimensions. To produce the +estimate, a [DOPE](https://github.com/NVlabs/Deep_Object_Pose) (Deep +Object Pose Estimation) pre-trained model is required. Input images may +need to be cropped and resized to maintain the aspect ratio and match +the input resolution of DOPE. After DNN inference has produced an estimate, the +DNN decoder will use the specified object type, along with the belief +maps produced by model inference, to output object poses. + +NVLabs has provided a DOPE pre-trained model using the +[HOPE](https://github.com/swtyree/hope-dataset) dataset. HOPE stands +for `Household Objects for Pose Estimation`. HOPE is a research-oriented +dataset that uses toy grocery objects and 3D textured meshes of the objects +for training on synthetic data. To use DOPE for other objects that are +relevant to your application, the model needs to be trained with another +dataset targeting these objects. For example, DOPE has been trained to +detect dollies for use with a mobile robot that navigates under, lifts, +and moves that type of dolly. To train your own DOPE model, please refer to the +[Training your Own DOPE Model Tutorial](https://nvidia-isaac-ros.github.io/concepts/pose_estimation/dope/tutorial_custom_model.html). + +`isaac_ros_centerpose` has similarities to `isaac_ros_dope` in that +both estimate an object pose; however, `isaac_ros_centerpose` provides +additional functionality. The +[CenterPose](https://github.com/NVlabs/CenterPose) DNN performs +object detection on the image, generates 2D keypoints for the object, +estimates the 6-DoF pose up to a scale, and regresses relative 3D bounding cuboid +dimensions. This is performed on a known object class without knowing +the instance-for example, a CenterPose model can detect a chair without having trained on +images of that specific chair. + +Pose estimation is a compute-intensive task and therefore not performed at the +frame rate of an input camera. To make efficient use of resources, +object pose is estimated for a single frame and used as an input to +navigation. Additional object pose estimates are computed to further +refine navigation in progress at a lower frequency than the input rate +of a typical camera. + +Packages in this repository rely on accelerated DNN model inference +using [Triton](https://github.com/triton-inference-server/server) or +[TensorRT](https://developer.nvidia.com/tensorrt) from [Isaac ROS DNN Inference](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_dnn_inference). +For preprocessing, packages in this rely on the `Isaac ROS DNN Image Encoder`, +which can also be found at [Isaac ROS DNN Inference](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_dnn_inference/blob/main/isaac_ros_dnn_image_encoder). ## Performance -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 4060 Ti | -| ---------------------------------------------------------------------------------------------------------------------------------------------------- | ---------- | ----------------------------------------------------------------------------------------------------------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------- | -| [DOPE Pose Estimation Graph](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/scripts//isaac_ros_dope_graph.py) | VGA | [40.5 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_dope_graph-agx_orin.json)
31 ms | [17.6 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_dope_graph-orin_nx.json)
120 ms | -- | [90.9 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_dope_graph-nuc_4060ti.json)
14 ms | -| [Centerpose Pose Estimation Graph](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/scripts//isaac_ros_centerpose_graph.py) | VGA | [50.2 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_centerpose_graph-agx_orin.json)
37 ms | [23.7 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_centerpose_graph-orin_nx.json)
70 ms | [18.4 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_centerpose_graph-orin_nano_emul.json)
87 ms | [45.0 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_centerpose_graph-nuc_4060ti.json)
21 ms | - -## Table of Contents - -- [Isaac ROS Pose Estimation](#isaac-ros-pose-estimation) - - [Overview](#overview) - - [Performance](#performance) - - [Table of Contents](#table-of-contents) - - [Latest Update](#latest-update) - - [Supported Platforms](#supported-platforms) - - [Docker](#docker) - - [Quickstart](#quickstart) - - [Next Steps](#next-steps) - - [Try More Examples](#try-more-examples) - - [Use Different Models](#use-different-models) - - [Customize your Dev Environment](#customize-your-dev-environment) - - [Package Reference](#package-reference) - - [`isaac_ros_dope`](#isaac_ros_dope) - - [Usage](#usage) - - [ROS Parameters](#ros-parameters) - - [Configuration File](#configuration-file) - - [ROS Topics Subscribed](#ros-topics-subscribed) - - [ROS Topics Published](#ros-topics-published) - - [`isaac_ros_centerpose`](#isaac_ros_centerpose) - - [Usage](#usage-1) - - [ROS Parameters](#ros-parameters-1) - - [Configuration File](#configuration-file-1) - - [ROS Topics Subscribed](#ros-topics-subscribed-1) - - [ROS Topics Published](#ros-topics-published-1) - - [CenterPose Network Output](#centerpose-network-output) - - [Troubleshooting](#troubleshooting) - - [Isaac ROS Troubleshooting](#isaac-ros-troubleshooting) - - [Deep Learning Troubleshooting](#deep-learning-troubleshooting) - - [Updates](#updates) - -## Latest Update - -Update 2023-05-25: Performance improvements. - -## Supported Platforms - -This package is designed and tested to be compatible with ROS 2 Humble running on [Jetson](https://developer.nvidia.com/embedded-computing) or an x86_64 system with an NVIDIA GPU. - -> **Note**: Versions of ROS 2 earlier than Humble are **not** supported. This package depends on specific ROS 2 implementation features that were only introduced beginning with the Humble release. - -| Platform | Hardware | Software | Notes | -| -------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| Jetson | [Jetson Orin](https://www.nvidia.com/en-us/autonomous-machines/embedded-systems/jetson-orin/)
[Jetson Xavier](https://www.nvidia.com/en-us/autonomous-machines/embedded-systems/jetson-agx-xavier/) | [JetPack 5.1.1](https://developer.nvidia.com/embedded/jetpack) | For best performance, ensure that [power settings](https://docs.nvidia.com/jetson/archives/r34.1/DeveloperGuide/text/SD/PlatformPowerAndPerformance.html) are configured appropriately. | -| x86_64 | NVIDIA GPU | [Ubuntu 20.04+](https://releases.ubuntu.com/20.04/)
[CUDA 11.8+](https://developer.nvidia.com/cuda-downloads) | - -### Docker - -To simplify development, we strongly recommend leveraging the Isaac ROS Dev Docker images by following [these steps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/docs/dev-env-setup.md). This will streamline your development environment setup with the correct versions of dependencies on both Jetson and x86_64 platforms. - -> **Note**: All Isaac ROS Quickstarts, tutorials, and examples have been designed with the Isaac ROS Docker images as a prerequisite. - -## Quickstart - -> **Warning**: Step 7 must be performed on `x86_64`. The resultant model should be copied over to the `Jetson`. Also note that the process of model preparation differs significantly from the other repositories. - -1. Set up your development environment by following the instructions [here](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/docs/dev-env-setup.md). -2. Clone this repository and its dependencies under `~/workspaces/isaac_ros-dev/src`. - - ```bash - cd ~/workspaces/isaac_ros-dev/src - ``` - - ```bash - git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common - ``` - - ```bash - git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_nitros - ``` - - ```bash - git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_pose_estimation - ``` - - ```bash - git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_dnn_inference - ``` - - ```bash - git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_image_pipeline - ``` - -3. Pull down a ROS Bag of sample data: - - ```bash - cd ~/workspaces/isaac_ros-dev/src/isaac_ros_pose_estimation && \ - git lfs pull -X "" -I "resources/rosbags/" - ``` - -4. Launch the Docker container using the `run_dev.sh` script: - - ```bash - cd ~/workspaces/isaac_ros-dev/src/isaac_ros_common && \ - ./scripts/run_dev.sh - ``` - -5. Make a directory to place models (inside the Docker container): - - ```bash - mkdir -p /tmp/models/ - ``` - -6. Select a DOPE model by visiting the DOPE model collection available on the official [DOPE GitHub](https://github.com/NVlabs/Deep_Object_Pose) repository [here](https://drive.google.com/open?id=1DfoA3m_Bm0fW8tOWXGVxi4ETlLEAgmcg). The model is assumed to be downloaded to `~/Downloads` outside the Docker container. - - This example will use `Ketchup.pth`, which should be downloaded into `/tmp/models` inside the Docker container: - > **Note**: this should be run outside the Docker container - - On `x86_64`: - - ```bash - cd ~/Downloads && \ - docker cp Ketchup.pth isaac_ros_dev-x86_64-container:/tmp/models - ``` - -7. Convert the PyTorch file into an ONNX file: - > **Warning**: this step must be performed on `x86_64`. The resultant model will be assumed to have been copied to the `Jetson` in the same output location (`/tmp/models/Ketchup.onnx`) - - ```bash - python3 /workspaces/isaac_ros-dev/src/isaac_ros_pose_estimation/isaac_ros_dope/scripts/dope_converter.py --format onnx --input /tmp/models/Ketchup.pth - ``` - - If you are planning on using Jetson, copy the generated `.onnx` model into the Jetson, and then copy it over into `aarch64` Docker container. - - We will assume that you already performed the transfer of the model onto the Jetson in the directory `~/Downloads`. - - Enter the Docker container in Jetson: - - ```bash - cd ~/workspaces/isaac_ros-dev/src/isaac_ros_common && \ - ./scripts/run_dev.sh - ``` - - Make a directory called `/tmp/models` in Jetson: - - ```bash - mkdir -p /tmp/models - ``` - - **Outside** the container, copy the generated `onnx` model: - - ```bash - cd ~/Downloads && \ - docker cp Ketchup.onnx isaac_ros_dev-aarch64-container:/tmp/models - ``` - -8. Inside the container, build and source the workspace: - - ```bash - cd /workspaces/isaac_ros-dev && \ - colcon build --symlink-install && \ - source install/setup.bash - ``` - -9. (Optional) Run tests to verify complete and correct installation: - - ```bash - colcon test --executor sequential - ``` - -10. Run the following launch files to spin up a demo of this package: - - Launch `isaac_ros_dope`: - - ```bash - ros2 launch isaac_ros_dope isaac_ros_dope_tensor_rt.launch.py model_file_path:=/tmp/models/Ketchup.onnx engine_file_path:=/tmp/models/Ketchup.plan - ``` - - Then open **another** terminal, and enter the Docker container again: - - ```bash - cd ~/workspaces/isaac_ros-dev/src/isaac_ros_common && \ - ./scripts/run_dev.sh - ``` - - Then, play the ROS bag: - - ```bash - ros2 bag play -l src/isaac_ros_pose_estimation/resources/rosbags/dope_rosbag/ - ``` - -11. Open another terminal window and attach to the same container. You should be able to get the poses of the objects in the images through `ros2 topic echo`: - - In a **third** terminal, enter the Docker container again: - - ```bash - cd ~/workspaces/isaac_ros-dev/src/isaac_ros_common && \ - ./scripts/run_dev.sh - ``` - - ```bash - ros2 topic echo /poses - ``` - - > **Note**: We are echoing `/poses` because we remapped the original topic `/dope/pose_array` to `poses` in the launch file. - - Now visualize the pose array in rviz2: - - ```bash - rviz2 - ``` - - Then click on the `Add` button, select `By topic` and choose `PoseArray` under `/poses`. Finally, change the display to show an axes by updating `Shape` to be `Axes`, as shown in the screenshot below. Make sure to update the `Fixed Frame` to `camera`. - -
- - > **Note**: For best results, crop or resize input images to the same dimensions your DNN model is expecting. - -## Next Steps - -### Try More Examples - -To continue your exploration, check out the following suggested examples: - -- [`DOPE` with `Triton`](docs/dope-triton.md) -- [`Centerpose` with `Triton`](docs/centerpose.md) -- [`DOPE` with non-standard input image sizes](docs/dope-custom-size.md) -- [Train your own `DOPE` model](docs/dope-custom-model.md) - -### Use Different Models - -Click [here](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_dnn_inference/blob/main/docs/model-preparation.md) for more information about how to use NGC models. - -Alternatively, consult the `DOPE` or `CenterPose` model repositories to try other models. - -| Model Name | Use Case | -| -------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------- | -| [DOPE](https://drive.google.com/open?id=1DfoA3m_Bm0fW8tOWXGVxi4ETlLEAgmcg) | The DOPE model repository. This should be used if `isaac_ros_dope` is used | -| [Centerpose](https://drive.google.com/drive/folders/1QIxcfKepOR4aktOz62p3Qag0Fhm0LVa0) | The Centerpose model repository. This should be used if `isaac_ros_centerpose` is used | - -### Customize your Dev Environment - -To customize your development environment, reference [this guide](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/README.md). - -## Package Reference - -### `isaac_ros_dope` - -#### Usage - -```bash -ros2 launch isaac_ros_dope isaac_ros_dope_tensor_rt.launch.py network_image_width:= network_image_height:= -model_file_path:= -engine_file_path:= input_tensor_names:= input_binding_names:= input_tensor_formats:= output_tensor_names:= output_binding_names:= output_tensor_formats:= -tensorrt_verbose:= object_name:= -``` - -> **Note**: there is also a `config` file that should be modified in `isaac_ros_dope/config/dope_config.yaml`. - -#### ROS Parameters - -| ROS Parameter | Type | Default | Description | -| -------------------- | -------- | ------------------ | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `configuration_file` | `string` | `dope_config.yaml` | The name of the configuration file to parse. Note: The node will look for that file name under isaac_ros_dope/config | -| `object_name` | `string` | `Ketchup` | The object class the DOPE network is detecting and the DOPE decoder is interpreting. This name should be listed in the configuration file along with its corresponding cuboid dimensions. | - -#### Configuration File - -The DOPE configuration file, which can be found at `isaac_ros_dope/config/dope_config.yaml` may need to modified. Specifically, you will need to specify an object type in the `DopeDecoderNode` that is listed in the `dope_config.yaml` file, so the DOPE decoder node will pick the right parameters to transform the belief maps from the inference node to object poses. The `dope_config.yaml` file uses the camera intrinsics of Realsense by default - if you are using a different camera, you will need to modify the camera_matrix field with the new, scaled `(640x480)` camera intrinsics. - -> **Note**: The `object_name` should correspond to one of the objects listed in the DOPE configuration file, with the corresponding model used. - -#### ROS Topics Subscribed - -| ROS Topic | Interface | Description | -| ------------------ | ----------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------ | -| `belief_map_array` | [isaac_ros_tensor_list_interfaces/TensorList](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/isaac_ros_tensor_list_interfaces/msg/TensorList.msg) | The tensor that represents the belief maps, which are outputs from the DOPE network. | - -#### ROS Topics Published - -| ROS Topic | Interface | Description | -| ----------------- | ---------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------- | -| `dope/pose_array` | [geometry_msgs/PoseArray](https://github.com/ros2/common_interfaces/blob/humble/geometry_msgs/msg/PoseArray.msg) | An array of poses of the objects detected by the DOPE network and interpreted by the DOPE decoder node. | - -### `isaac_ros_centerpose` - -#### Usage - -```bash -ros2 launch isaac_ros_centerpose isaac_ros_centerpose.launch.py network_image_width:= network_image_height:= encoder_image_mean:= encoder_image_stddev:= -model_name:= -model_repository_paths:= max_batch_size:= input_tensor_names:= input_binding_names:= input_tensor_formats:= output_tensor_names:= output_binding_names:= output_tensor_formats:= -``` - -> **Note**: there is also a `config` file that should be modified in `isaac_ros_centerpose/config/decoders_param.yaml`. - -#### ROS Parameters - -| ROS Parameter | Type | Default | Description | -| --------------------- | ------------ | ------------------------------------------------------------------------------------------------ | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `camera_matrix` | `float list` | `[616.078125, 0.0, 325.8349304199219, 0.0, 616.1030883789062, 244.4612274169922, 0.0, 0.0, 1.0]` | A row-major array of 9 floats that represent the camera intrinsics matrix `K`. | -| `original_image_size` | `float list` | `[640, 480]` | An array of two floats that represent the size of the original image passed into the image encoder. The first element needs to be width, and the second element needs to be height. | -| `output_field_size` | `int list` | `[128, 128]` | An array of two integers that represent the size of the 2D keypoint decoding from the network output | -| `height` | `float` | `0.1` | This parameter is used to scale the cuboid used for calculating the size of the objects detected. | -| `frame_id` | `string` | `centerpose` | The frame ID that the DOPE decoder node will write to the header of its output messages | -| `marker_color` | `float list` | `[1.0, 0.0, 0.0, 1.0]` (red) | An array of 4 floats representing RGBA that will be used to define the color that will be used by RViz to visualize the marker. Each value should be between 0.0 and 1.0. | - -#### Configuration File - -The default parameters for the `CenterPoseDecoderNode` is defined in the `decoders_param.yaml` file under `isaac_ros_centerpose/config`. The `decoders_param.yaml` file uses the camera intrinsics of RealSense by default - if you are using a different camera, you will need to modify the `camera_matrix` field. - -#### ROS Topics Subscribed - -| ROS Topic | Interface | Description | -| ------------ | ----------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------- | -| `tensor_sub` | [isaac_ros_tensor_list_interfaces/TensorList](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/isaac_ros_tensor_list_interfaces/msg/TensorList.msg) | The TensorList that contains the outputs of the CenterPose network. | - -#### ROS Topics Published - -| ROS Topic | Interface | Description | -| -------------- | ------------------------------------------------------------------------------------------------------------------------------ | ------------------------------------------------------------------------------------------------------------------------------------ | -| `object_poses` | [visualization_msgs/MarkerArray](https://github.com/ros2/common_interfaces/blob/humble/visualization_msgs/msg/MarkerArray.msg) | A `MarkerArray` representing the poses of objects detected by the CenterPose network and interpreted by the CenterPose decoder node. | - -#### CenterPose Network Output - -The CenterPose network has 7 different outputs: -| Output Name | Meaning | -| ----------- | ------------------------------- | -| `hm` | Object center heatmap | -| `wh` | 2D bounding box size | -| `hps` | Keypoint displacements | -| `reg` | Sub-pixel offset | -| `hm_hp` | Keypoint heatmaps | -| `hp_offset` | Sub-pixel offsets for keypoints | -| `scale` | Relative cuboid dimensions | +| Sample Graph

| Input Size

| AGX Orin

| Orin NX

| Orin Nano 8GB

| x86_64 w/ RTX 4060 Ti

| +|-----------------------------------------------------------------------------------------------------------------------------------------------------------------|-------------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------------|----------------------------------------------------------------------------------------------------------------------------------------------------------------| +| [DOPE Pose Estimation Graph](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/scripts/isaac_ros_dope_graph.py)



| VGA



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


33 ms

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


120 ms

| –



| [89.2 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_dope_graph-nuc_4060ti.json)


15 ms

| +| [Centerpose Pose Estimation Graph](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/scripts/isaac_ros_centerpose_graph.py)



| VGA



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


5.7 ms

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


7.4 ms

| [13.8 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_centerpose_graph-orin_nano.json)


12 ms

| [50.2 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_centerpose_graph-nuc_4060ti.json)


14 ms

| -For more context and explanation, see the corresponding outputs in Figure 2 of the CenterPose [paper](https://arxiv.org/pdf/2109.06161.pdf) and refer to the paper. +--- -## Troubleshooting +## Documentation -### Isaac ROS Troubleshooting +Please visit the [Isaac ROS Documentation](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_pose_estimation/index.html) to learn how to use this repository. -For solutions to problems with Isaac ROS, please check [here](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/docs/troubleshooting.md). +--- -### Deep Learning Troubleshooting +## Packages -For solutions to problems with using DNN models, please check [here](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_dnn_inference/blob/main/docs/troubleshooting.md). +* [`isaac_ros_centerpose`](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_pose_estimation/isaac_ros_centerpose/index.html) + * [Quickstart](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_pose_estimation/isaac_ros_centerpose/index.html#quickstart) + * [Troubleshooting](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_pose_estimation/isaac_ros_centerpose/index.html#troubleshooting) + * [API](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_pose_estimation/isaac_ros_centerpose/index.html#api) +* [`isaac_ros_dope`](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_pose_estimation/isaac_ros_dope/index.html) + * [Quickstart](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_pose_estimation/isaac_ros_dope/index.html#quickstart) + * [Try More Examples](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_pose_estimation/isaac_ros_dope/index.html#try-more-examples) + * [Use Different Models](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_pose_estimation/isaac_ros_dope/index.html#use-different-models) + * [Troubleshooting](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_pose_estimation/isaac_ros_dope/index.html#troubleshooting) + * [API](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_pose_estimation/isaac_ros_dope/index.html#api) -## Updates +## Latest -| Date | Changes | -| ---------- | -------------------------------------------------------------------------------------------------------- | -| 2023-05-25 | Performance improvements | -| 2023-04-05 | Source available GXF extensions | -| 2022-06-30 | Update to use NITROS for improved performance and to be compatible with JetPack 5.0.2 | -| 2022-06-30 | Refactored README, updated launch file & added `nvidia` namespace, dropped Jetson support for CenterPose | -| 2021-10-20 | Initial update | +Update 2023-10-18: Adding NITROS CenterPose decoder. diff --git a/docs/centerpose.md b/docs/centerpose.md deleted file mode 100644 index 00d31c0..0000000 --- a/docs/centerpose.md +++ /dev/null @@ -1,88 +0,0 @@ -# Tutorial for CenterPose Inference using Triton - -
- -## Overview - -This tutorial walks you through a graph to estimate the 6DOF pose of a target object using [CenterPose](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_pose_estimation) with Triton. It uses input monocular images from a rosbag. -> **Warning**: These steps will only work on `x86_64` and **NOT** on `Jetson`. - -## Tutorial Walkthrough - -1. Complete steps 1-5 of the quickstart [here](../README.md#quickstart) -2. Select a CenterPose model by visiting the CenterPose model collection available on the official [CenterPose GitHub](https://github.com/NVlabs/CenterPose) repository [here](https://drive.google.com/drive/folders/1QIxcfKepOR4aktOz62p3Qag0Fhm0LVa0). The model is assumed to be downloaded to `~/Downloads` outside the docker container. This example will use `shoe_resnet_140.pth`, which should be downloaded into `/tmp/models` inside the docker container: - > **Note**: this should be run outside the container - - ```bash - cd ~/Downloads && \ - docker cp shoe_resnet_140.pth isaac_ros_dev-x86_64-container:/tmp/models - ``` - - > **Warning**: The models in the root directory of the model collection listed above will *NOT WORK* with our inference nodes because they have custom layers not supported by TensorRT nor Triton. Make sure to use the PyTorch weights that have the string `resnet` in their file names. - -3. Create a models repository with version `1`: - - ```bash - mkdir -p /tmp/models/centerpose_shoe/1 - ``` - -4. Create a configuration file for this model at path `/tmp/models/centerpose_shoe/config.pbtxt`. Note that name has to be the same as the model repository name. Take a look at the example at `isaac_ros_centerpose/test/models/centerpose_shoe/config.pbtxt` and copy that file to `/tmp/models/centerpose_shoe/config.pbtxt`. - - ```bash - cp /workspaces/isaac_ros-dev/src/isaac_ros_pose_estimation/isaac_ros_centerpose/test/models/centerpose_shoe/config.pbtxt /tmp/models/centerpose_shoe/config.pbtxt - ``` - -5. To run the TensorRT engine plan, convert the PyTorch model to ONNX first. Export the model into an ONNX file using the script provided under `/workspaces/isaac_ros-dev/src/isaac_ros_pose_estimation/isaac_ros_centerpose/scripts/centerpose_pytorch2onnx.py`: - - ```bash - python3 /workspaces/isaac_ros-dev/src/isaac_ros_pose_estimation/isaac_ros_centerpose/scripts/centerpose_pytorch2onnx.py --input /tmp/models/shoe_resnet_140.pth --output /tmp/models/centerpose_shoe/1/model.onnx - ``` - -6. To get a TensorRT engine plan file with Triton, export the ONNX model into an TensorRT engine plan file using the builtin TensorRT converter `trtexec`: - - ```bash - /usr/src/tensorrt/bin/trtexec --onnx=/tmp/models/centerpose_shoe/1/model.onnx --saveEngine=/tmp/models/centerpose_shoe/1/model.plan - ``` - -7. Inside the container, build and source the workspace: - - ```bash - cd /workspaces/isaac_ros-dev && \ - colcon build --symlink-install && \ - source install/setup.bash - ``` - -8. Start `isaac_ros_centerpose` using the launch file: - - ```bash - ros2 launch isaac_ros_centerpose isaac_ros_centerpose.launch.py model_name:=centerpose_shoe model_repository_paths:=['/tmp/models'] - ``` - - Then open **another** terminal, and enter the Docker container again: - - ```bash - cd ~/workspaces/isaac_ros-dev/src/isaac_ros_common && \ - ./scripts/run_dev.sh - ``` - - Then, play the ROS bag: - - ```bash - ros2 bag play -l src/isaac_ros_pose_estimation/resources/rosbags/centerpose_rosbag/ - ``` - -9. Open another terminal window and attach to the same container. You should be able to get the poses of the objects in the images through `ros2 topic echo`: - - In a **third** terminal, enter the Docker container again: - - ```bash - cd ~/workspaces/isaac_ros-dev/src/isaac_ros_common && \ - ./scripts/run_dev.sh - ``` - - ```bash - source install/setup.bash && \ - ros2 topic echo /object_poses - ``` - -10. Launch `rviz2`. Click on `Add` button, select "By topic", and choose `MarkerArray` under `/object_poses`. Set the fixed frame to `centerpose`. You'll be able to see the cuboid marker representing the object's pose detected! diff --git a/docs/dope-custom-model.md b/docs/dope-custom-model.md deleted file mode 100644 index ada35e9..0000000 --- a/docs/dope-custom-model.md +++ /dev/null @@ -1,33 +0,0 @@ -# Training your own DOPE model - -## Overview - -The DOPE network architecture is intended to be trained on objects of a specific class, which means that using DOPE for pose estimation of a custom object class requires training a custom model for that class. - -[NVIDIA Isaac Sim](https://developer.nvidia.com/isaac-sim) offers a convenient workflow for training a custom DOPE model using synthetic data generation (SDG). - -## Tutorial Walkthrough - -1. Clone the [Isaac Sim DOPE Training repository](https://github.com/andrewyguo/dope_training#deep-object-pose-estimation-dope---training) and follow the training instructions to prepare a custom DOPE model. -2. Using the [Isaac Sim DOPE inference script](https://github.com/andrewyguo/dope_training/tree/master/inference), test the custom DOPE model's inference capability and ensure that the quality is acceptable for your use case. - -3. Follow steps 1-5 of the main DOPE [quickstart](../README.md#quickstart). - -4. At step 6, move the prepared `.pth` model output from the Isaac Sim DOPE Training script into the `/tmp/models` path inside the Docker container. - ```bash - docker cp custom_model.pth isaac_ros_dev-x86_64-container:/tmp/models - ``` -5. At step 7, run the `dope_converter.py` script with the custom model: - - ```bash - python3 /workspaces/isaac_ros-dev/src/isaac_ros_pose_estimation/isaac_ros_dope/scripts/dope_converter.py --format onnx --input /tmp/models/custom_model.pth - ``` - -6. Proceed through steps 8-9. -7. At step 10, launch the ROS 2 launchfile with the custom model: - - ```bash - ros2 launch isaac_ros_dope isaac_ros_dope_tensor_rt.launch.py model_file_path:=/tmp/models/custom_model.onnx engine_file_path:=/tmp/models/custom_model.plan - ``` - -8. Continue with the rest of the quickstart. You should now be able to detect poses of custom objects. diff --git a/docs/dope-custom-size.md b/docs/dope-custom-size.md deleted file mode 100644 index 82dbac1..0000000 --- a/docs/dope-custom-size.md +++ /dev/null @@ -1,26 +0,0 @@ -# Using DOPE with a non-standard input image size - -## Overview - -The DOPE network architecture, as outlined in the [original paper](https://arxiv.org/abs/1809.10790), can receive input images of arbitrary size and subsequently produce output belief maps of the corresponding dimensions. - -However, the ONNX format used to run this network on Triton or TensorRT is not as flexible, and an ONNX-exported model **does NOT** support arbitrary image sizes at inference time. Instead, the desired input image dimensions must be explicitly specified when preparing the ONNX file using the `dope_converter.py` script, as referenced in the [quickstart](../README.md#quickstart). - -## Tutorial Walkthrough - -1. Follow steps 1-6 of the main DOPE [quickstart](../README.md#quickstart). - -2. At step 7, run the `dope_converter.py` script with the two additional arguments `row` and `col` specifying the desired input image size: - - ```bash - python3 /workspaces/isaac_ros-dev/src/isaac_ros_pose_estimation/isaac_ros_dope/scripts/dope_converter.py --format onnx --input /tmp/models/Ketchup.pth --row 1080 --col 1920 - ``` - -3. Proceed through steps 8-9. -4. At step 10, launch the ROS 2 launchfile with two additional arguments `network_image_height` and `network_image_width` specifying the desired input image size: - - ```bash - ros2 launch isaac_ros_dope isaac_ros_dope_tensor_rt.launch.py model_file_path:=/tmp/models/Ketchup.onnx engine_file_path:=/tmp/models/Ketchup.plan network_image_height:=1080 network_image_width:=1920 - ``` - -5. Continue with the rest of the quickstart. You should now be able to detect poses in images of your desired size. diff --git a/docs/dope-triton.md b/docs/dope-triton.md deleted file mode 100644 index 309795f..0000000 --- a/docs/dope-triton.md +++ /dev/null @@ -1,128 +0,0 @@ -# Tutorial for DOPE Inference - -
- -## Overview - -This tutorial walks you through a graph to estimate the 6DOF pose of a target object using [DOPE](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_pose_estimation) using different backends. It uses input monocular images from a rosbag. The different backends show are: - -1. PyTorch and ONNX -2. TensorRT Plan files with Triton -3. PyTorch model with Triton - -> **Note**: The DOPE converter script only works on `x86_64`, so the resultant `onnx` model following these steps must be copied to the Jetson. - -## Tutorial Walkthrough - -1. Complete steps 1-6 of the quickstart [here](../README.md#quickstart). -2. Make a directory called `Ketchup` inside `/tmp/models`, which will serve as the model repository. This will be versioned as `1`. The downloaded model will be placed here: - - ```bash - mkdir -p /tmp/models/Ketchup/1 && \ - mv /tmp/models/Ketchup.pth /tmp/models/Ketchup/ - ``` - -3. Now select a backend. The PyTorch and ONNX options **MUST** be run on `x86_64`: - - To run ONNX models with Triton, export the model into an ONNX file using the script provided under `/workspaces/isaac_ros-dev/src/isaac_ros_pose_estimation/isaac_ros_dope/scripts/dope_converter.py`: - - ```bash - python3 /workspaces/isaac_ros-dev/src/isaac_ros_pose_estimation/isaac_ros_dope/scripts/dope_converter.py --format onnx --input /tmp/models/Ketchup/Ketchup.pth --output /tmp/models/Ketchup/1/model.onnx --input_name INPUT__0 --output_name OUTPUT__0 - ``` - - - To run `TensorRT Plan` files with Triton, first copy the generated `onnx` model from the above point to the target platform (e.g. a Jetson or an `x86_64` machine). The model will be assumed to be copied to `/tmp/models/Ketchup/1/model.onnx` inside the Docker container. Then use `trtexec` to convert the `onnx` model to a `plan` model: - - ```bash - /usr/src/tensorrt/bin/trtexec --onnx=/tmp/models/Ketchup/1/model.onnx --saveEngine=/tmp/models/Ketchup/1/model.plan - ``` - - - To run PyTorch model with Triton (**inferencing PyTorch model is supported for x86_64 platform only**), the model needs to be saved using `torch.jit.save()`. The downloaded DOPE model is saved with `torch.save()`. Export the DOPE model using the script provided under `/workspaces/isaac_ros-dev/src/isaac_ros_pose_estimation/isaac_ros_dope/scripts/dope_converter.py`: - - ```bash - python3 /workspaces/isaac_ros-dev/src/isaac_ros_pose_estimation/isaac_ros_dope/scripts/dope_converter.py --format pytorch --input /tmp/models/Ketchup/Ketchup.pth --output /tmp/models/Ketchup/1/model.pt - ``` - -4. Create a configuration file for this model at path `/tmp/models/Ketchup/config.pbtxt`. Note that name has to be the same as the model repository. Depending on the platform selected from a previous step, a slightly different `config.pbtxt` file must be created: `onnxruntime_onnx` (`.onnx` file), `tensorrt_plan` (`.plan` file) or `pytorch_libtorch` (`.pt` file): - - ```log - name: "Ketchup" - platform: - max_batch_size: 0 - input [ - { - name: "INPUT__0" - data_type: TYPE_FP32 - dims: [ 1, 3, 480, 640 ] - } - ] - output [ - { - name: "OUTPUT__0" - data_type: TYPE_FP32 - dims: [ 1, 25, 60, 80 ] - } - ] - version_policy: { - specific { - versions: [ 1 ] - } - } - ``` - - The `` part should be replaced with `onnxruntime_onnx` for `.onnx` files, `tensorrt_plan` for `.plan` files and `pytorch_libtorch` for `.pt` files. - - > **Note**: The DOPE decoder currently works with the output of a DOPE network that has a fixed input size of 640 x 480, which are the default dimensions set in the script. In order to use input images of other sizes, make sure to crop or resize using ROS 2 nodes from [Isaac ROS Image Pipeline](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_image_pipeline) or similar packages. - - > **Note**: The model name must be `model.onnx`. - -5. Rebuild and source `isaac_ros_dope`: - - ```bash - cd /workspaces/isaac_ros-dev - colcon build --packages-up-to isaac_ros_dope && source install/setup.bash - ``` - -6. Start `isaac_ros_dope` using the launch file: - - ```bash - ros2 launch isaac_ros_dope isaac_ros_dope_triton.launch.py model_name:=Ketchup model_repository_paths:=['/tmp/models'] input_binding_names:=['INPUT__0'] output_binding_names:=['OUTPUT__0'] object_name:=Ketchup - ``` - - > **Note**: `object_name` should correspond to one of the objects listed in the DOPE configuration file, and the specified model should be a DOPE model that is trained for that specific object. - -7. Open **another** terminal, and enter the Docker container again: - - ```bash - cd ~/workspaces/isaac_ros-dev/src/isaac_ros_common && \ - ./scripts/run_dev.sh - ``` - - Then, play the ROS bag: - - ```bash - ros2 bag play -l src/isaac_ros_pose_estimation/resources/rosbags/dope_rosbag/ - ``` - -8. Open another terminal window and attach to the same container. You should be able to get the poses of the objects in the images through `ros2 topic echo`: - - In a **third** terminal, enter the Docker container again: - - ```bash - cd ~/workspaces/isaac_ros-dev/src/isaac_ros_common && \ - ./scripts/run_dev.sh - ``` - - ```bash - ros2 topic echo /poses - ``` - - > **Note**: We are echoing `/poses` because we remapped the original topic `/dope/pose_array` to `poses` in the launch file. - - Now visualize the pose array in rviz2: - - ```bash - rviz2 - ``` - - Then click on the `Add` button, select `By topic` and choose `PoseArray` under `/poses`. Finally, change the display to show an axes by updating `Shape` to be `Axes`, as shown in the screenshot at the top of this page. Make sure to update the `Fixed Frame` to `camera`. - - > **Note**: For best results, crop/resize input images to the same dimensions your DNN model is expecting. diff --git a/isaac_ros_centerpose/CMakeLists.txt b/isaac_ros_centerpose/CMakeLists.txt index 0f15a6e..5c43432 100644 --- a/isaac_ros_centerpose/CMakeLists.txt +++ b/isaac_ros_centerpose/CMakeLists.txt @@ -1,5 +1,5 @@ # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2021-2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# 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. @@ -15,27 +15,45 @@ # # SPDX-License-Identifier: Apache-2.0 -cmake_minimum_required(VERSION 3.23.2) -project(isaac_ros_centerpose LANGUAGES PYTHON) +cmake_minimum_required(VERSION 3.22.1) +project(isaac_ros_centerpose LANGUAGES C CXX) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -find_package(ament_cmake REQUIRED) find_package(ament_cmake_auto REQUIRED) -find_package(ament_cmake_python REQUIRED) -find_package(rclpy REQUIRED) ament_auto_find_build_dependencies() -# Install Python modules -ament_python_install_package(${PROJECT_NAME}) +# Decoder node +ament_auto_add_library(centerpose_decoder_node SHARED src/centerpose_decoder_node.cpp) +rclcpp_components_register_nodes(centerpose_decoder_node "nvidia::isaac_ros::centerpose::CenterPoseDecoderNode") +set(node_plugins "${node_plugins}nvidia::isaac_ros::centerpose::CenterPoseDecoderNode;$\n") -# Install Python executables -install(PROGRAMS - isaac_ros_centerpose/CenterPoseDecoder.py - isaac_ros_centerpose/CenterPoseDecoderUtils.py - DESTINATION lib/${PROJECT_NAME} -) +# Visualizer node +ament_auto_add_library(centerpose_visualizer_node SHARED src/centerpose_visualizer_node.cpp) +rclcpp_components_register_nodes(centerpose_visualizer_node "nvidia::isaac_ros::centerpose::CenterPoseVisualizerNode") +set(node_plugins "${node_plugins}nvidia::isaac_ros::centerpose::CenterPoseVisualizerNode;$\n") -ament_auto_package() +### Install extensions built from source + +# CenterPose +add_subdirectory(gxf/centerpose) +install(TARGETS gxf_centerpose DESTINATION share/${PROJECT_NAME}/gxf/lib/centerpose) + +### End extensions +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + # The FindPythonInterp and FindPythonLibs modules are removed + if(POLICY CMP0148) + cmake_policy(SET CMP0148 OLD) + endif() + + find_package(launch_testing_ament_cmake REQUIRED) + add_launch_test(test/test_centerpose_pol.py TIMEOUT "600") + add_launch_test(test/test_centerpose_pol_triton.py TIMEOUT "600") +endif() + +ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/isaac_ros_centerpose/config/centerpose_node.yaml b/isaac_ros_centerpose/config/centerpose_node.yaml new file mode 100644 index 0000000..a1fcd43 --- /dev/null +++ b/isaac_ros_centerpose/config/centerpose_node.yaml @@ -0,0 +1,127 @@ +%YAML 1.2 +# 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 +--- +name: centerpose_decoder +components: +- name: input + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 12 + policy: 0 +- name: camera_model_input + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 12 + policy: 0 +- name: output + type: nvidia::gxf::DoubleBufferTransmitter + parameters: + capacity: 12 + policy: 0 +- name: allocator + type: nvidia::gxf::UnboundedAllocator +- name: centerpose_decoder + type: nvidia::isaac::centerpose::CenterPosePostProcessor + parameters: + input: input + camera_model_input: camera_model_input + output: output + allocator: allocator + output_field_size: [] + cuboid_scaling_factor: 0.0 + score_threshold: 1.0 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: input + min_size: 1 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: camera_model_input + min_size: 1 +- type: nvidia::gxf::DownstreamReceptiveSchedulingTerm + parameters: + transmitter: output + min_size: 1 +--- +name: centerpose_decoder_to_isaac +components: +- name: input + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 12 + policy: 0 +- name: output + type: nvidia::gxf::DoubleBufferTransmitter + parameters: + capacity: 12 + policy: 0 +- name: centerpose_decoder_to_isaac + type: nvidia::isaac::centerpose::CenterPoseDetectionToIsaac + parameters: + input: input + output: output + object_name: "" +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: input + min_size: 1 +- type: nvidia::gxf::DownstreamReceptiveSchedulingTerm + parameters: + transmitter: output + min_size: 1 +--- +name: sink +components: +- name: signal + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 2 + policy: 0 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: signal + min_size: 1 +- name: sink + type: nvidia::isaac_ros::MessageRelay + parameters: + source: signal +--- +components: +- name: edge0 + type: nvidia::gxf::Connection + parameters: + source: centerpose_decoder/output + target: centerpose_decoder_to_isaac/input +- name: edge1 + type: nvidia::gxf::Connection + parameters: + source: centerpose_decoder_to_isaac/output + target: sink/signal +--- +components: +- name: clock + type: nvidia::gxf::RealtimeClock +- 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_centerpose/config/centerpose_visualizer_node.yaml b/isaac_ros_centerpose/config/centerpose_visualizer_node.yaml new file mode 100644 index 0000000..4a07eb8 --- /dev/null +++ b/isaac_ros_centerpose/config/centerpose_visualizer_node.yaml @@ -0,0 +1,104 @@ +%YAML 1.2 +# 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 +--- +name: centerpose_visualizer +components: +- name: video_buffer_input + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 12 + policy: 0 +- name: detections_input + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 12 + policy: 0 +- name: camera_model_input + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 12 + policy: 0 +- name: output + type: nvidia::gxf::DoubleBufferTransmitter + parameters: + capacity: 12 + policy: 0 +- name: allocator + type: nvidia::gxf::UnboundedAllocator +- name: centerpose_visualizer + type: nvidia::isaac::centerpose::CenterPoseVisualizer + parameters: + video_buffer_input: video_buffer_input + detections_input: detections_input + camera_model_input: camera_model_input + output: output + allocator: allocator + show_axes: true + bounding_box_color: 0x00 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: video_buffer_input + min_size: 1 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: detections_input + min_size: 1 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: camera_model_input + min_size: 1 +- type: nvidia::gxf::DownstreamReceptiveSchedulingTerm + parameters: + transmitter: output + min_size: 1 +--- +name: sink +components: +- name: signal + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 2 + policy: 0 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: signal + min_size: 1 +- name: sink + type: nvidia::isaac_ros::MessageRelay + parameters: + source: signal +--- +components: +- name: edge0 + type: nvidia::gxf::Connection + parameters: + source: centerpose_visualizer/output + target: sink/signal +--- +components: +- name: clock + type: nvidia::gxf::RealtimeClock +- 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_centerpose/config/decoder_params.yaml b/isaac_ros_centerpose/config/decoder_params.yaml deleted file mode 100644 index 9b545a2..0000000 --- a/isaac_ros_centerpose/config/decoder_params.yaml +++ /dev/null @@ -1,26 +0,0 @@ -%YAML 1.2 -# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2021-2022 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 ---- -centerpose_decoder_node: - ros__parameters: - camera_matrix: [616.078125, 0.0, 325.8349304199219, 0.0, 616.1030883789062, 244.4612274169922, 0.0, 0.0, 1.0] - original_image_size: [640, 480] - output_field_size: [128, 128] - height: 0.1 - frame: centerpose - marker_color: [1.0, 0.0, 0.0, 1.0] diff --git a/isaac_ros_centerpose/config/decoder_params_test.yaml b/isaac_ros_centerpose/config/decoder_params_test.yaml deleted file mode 100644 index d00b22f..0000000 --- a/isaac_ros_centerpose/config/decoder_params_test.yaml +++ /dev/null @@ -1,27 +0,0 @@ -%YAML 1.2 -# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2021-2022 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 ---- -isaac_ros_test: - centerpose_decoder_node: - ros__parameters: - camera_matrix: [616.078125, 0.0, 325.8349304199219, 0.0, 616.1030883789062, 244.4612274169922, 0.0, 0.0, 1.0] - original_image_size: [640, 480] - output_field_size: [128, 128] - height: 0.1 - frame_id: centerpose - marker_color: [1.0, 0.0, 0.0, 1.0] diff --git a/isaac_ros_centerpose/isaac_ros_centerpose/__init__.py b/isaac_ros_centerpose/gxf/AMENT_IGNORE similarity index 100% rename from isaac_ros_centerpose/isaac_ros_centerpose/__init__.py rename to isaac_ros_centerpose/gxf/AMENT_IGNORE diff --git a/isaac_ros_centerpose/gxf/centerpose/CMakeLists.txt b/isaac_ros_centerpose/gxf/centerpose/CMakeLists.txt new file mode 100644 index 0000000..ff61a79 --- /dev/null +++ b/isaac_ros_centerpose/gxf/centerpose/CMakeLists.txt @@ -0,0 +1,68 @@ +# 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_centerpose LANGUAGES C CXX) + +# Dependencies +find_package(CUDAToolkit REQUIRED) +find_package (Eigen3 3.3 REQUIRED) +find_package(GXF ${ISAAC_ROS_GXF_VERSION} MODULE REQUIRED + COMPONENTS + cuda + multimedia + std + isaac_messages +) +find_package(OpenCV REQUIRED) +find_package(yaml-cpp) +find_package(isaac_ros_nitros_detection3_d_array_type REQUIRED) + +# Centerpose extension +add_library(gxf_centerpose SHARED + components/centerpose_postprocessor.cpp + components/centerpose_postprocessor.hpp + components/cuboid3d.hpp + components/cuboid3d.cpp + components/cuboid_pnp_solver.cpp + components/cuboid_pnp_solver.hpp + components/centerpose_detection.hpp + components/centerpose_detection_to_isaac.hpp + components/centerpose_detection_to_isaac.cpp + components/soft_nms_nvidia.cpp + components/soft_nms_nvidia.hpp + components/centerpose_visualizer.cpp + components/centerpose_visualizer.hpp + components/video_buffer_utils.hpp + centerpose_ext.cpp +) +target_include_directories(gxf_centerpose + PUBLIC + ${OpenCV_INCLUDE_DIRS} + ${isaac_ros_nitros_detection3_d_array_type_INCLUDE_DIRS} +) +target_link_libraries(gxf_centerpose + PUBLIC + CUDA::cudart + Eigen3::Eigen + GXF::cuda + GXF::isaac_messages + GXF::multimedia + GXF::std + ${OpenCV_LIBS} + yaml-cpp + ${isaac_ros_nitros_detection3_d_array_type_LIBRARIES} +) diff --git a/isaac_ros_centerpose/gxf/centerpose/centerpose_ext.cpp b/isaac_ros_centerpose/gxf/centerpose/centerpose_ext.cpp new file mode 100644 index 0000000..12e24d2 --- /dev/null +++ b/isaac_ros_centerpose/gxf/centerpose/centerpose_ext.cpp @@ -0,0 +1,52 @@ +// 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 "components/centerpose_detection_to_isaac.hpp" +#include "components/centerpose_postprocessor.hpp" +#include "components/centerpose_visualizer.hpp" +#include "detection3_d_array_message.hpp" +#include "gxf/std/extension_factory_helper.hpp" + +GXF_EXT_FACTORY_BEGIN() + +GXF_EXT_FACTORY_SET_INFO( + 0xc4091ff8477411ee, 0xa4e6036b2c3e1879, "CenterPose", + "Extension containing CenterPose pose estimation related components", "Isaac SDK", "2.0.0", + "LICENSE"); + +GXF_EXT_FACTORY_ADD( + 0xdaf98ba8477411ee, 0x93ba3f47723d071c, nvidia::isaac::centerpose::CenterPosePostProcessor, + nvidia::gxf::Codelet, + "Generates pose estimations given an output from the CenterPose neural network"); + +GXF_EXT_FACTORY_ADD( + 0x6c40d78e528c11ee, 0x990daf0e3d1195a0, nvidia::isaac::centerpose::CenterPoseDetectionToIsaac, + nvidia::gxf::Codelet, + "Converts pose estimations from the CenterPosePostProcessor into a Isaac-friendly format"); + +GXF_EXT_FACTORY_ADD( + 0x0e92a75e617111ee, 0x8c97b3583a2280c4, nvidia::isaac::centerpose::CenterPoseVisualizer, + nvidia::gxf::Codelet, + "Visualizes results of centerpose detections by projecting it onto images"); + +GXF_EXT_FACTORY_ADD_0( + 0x65d4476051a511ee, 0xb3f727d2ed955144, nvidia::isaac::ObjectHypothesis, + "List of scores and class ids"); + +GXF_EXT_FACTORY_ADD_0( + 0x782823c851dc11ee, 0xa5dc87b46496e7b8, nvidia::isaac::Vector3f, "3 Dimensional Vector"); + +GXF_EXT_FACTORY_END() diff --git a/isaac_ros_centerpose/gxf/centerpose/components/centerpose_detection.hpp b/isaac_ros_centerpose/gxf/centerpose/components/centerpose_detection.hpp new file mode 100644 index 0000000..939446d --- /dev/null +++ b/isaac_ros_centerpose/gxf/centerpose/components/centerpose_detection.hpp @@ -0,0 +1,44 @@ +// 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 "centerpose_types.hpp" +#include "Eigen/Dense" + +namespace nvidia { +namespace isaac { +namespace centerpose { +struct CenterPoseDetection { + int64_t class_id; + Eigen::MatrixXfRM keypoints2d; + Eigen::MatrixXfRM projected_keypoints_2d; + Eigen::MatrixXfRM keypoints3d; + Eigen::Vector3f position; + Eigen::Quaternionf quaternion; + float score; + Eigen::Vector3f bbox_size; + Eigen::MatrixXfRM bbox; + Eigen::MatrixXfRM kps_heatmap_mean; +}; + +using CenterPoseDetectionList = std::vector; + +} // namespace centerpose +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_centerpose/gxf/centerpose/components/centerpose_detection_to_isaac.cpp b/isaac_ros_centerpose/gxf/centerpose/components/centerpose_detection_to_isaac.cpp new file mode 100644 index 0000000..800a38a --- /dev/null +++ b/isaac_ros_centerpose/gxf/centerpose/components/centerpose_detection_to_isaac.cpp @@ -0,0 +1,219 @@ +// 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 "centerpose_detection_to_isaac.hpp" + +#include "detection3_d_array_message.hpp" +#include "gxf/std/tensor.hpp" + +namespace nvidia { +namespace isaac { +namespace centerpose { +namespace { + +template +struct PointerView { + PointerView() : data{}, size{} {} + PointerView(const std::vector& vec) : data{vec.data()}, size{vec.size()} {} + + PointerView(gxf::Handle tensor) + : data{tensor->data().value()}, size{tensor->size() / sizeof(T)} {} + + const T* data{}; + size_t size{}; +}; + +template +gxf::Expected> ToVector(gxf::Handle tensor) { + std::vector vec; + vec.resize(tensor->size() / sizeof(T)); + switch (tensor->storage_type()) { + case gxf::MemoryStorageType::kHost: + case gxf::MemoryStorageType::kSystem: { + std::memcpy(vec.data(), tensor->pointer(), tensor->size()); + } break; + case gxf::MemoryStorageType::kDevice: { + const cudaError_t cuda_error = + cudaMemcpy(vec.data(), tensor->pointer(), tensor->size(), cudaMemcpyDeviceToHost); + if (cuda_error != cudaSuccess) { + GXF_LOG_ERROR( + "Failed to transfer memory from device to host: %s", cudaGetErrorString(cuda_error)); + return gxf::Unexpected{GXF_FAILURE}; + } + } break; + default: + GXF_LOG_ERROR("Recieved unexpected MemoryStorageType"); + return gxf::Unexpected{GXF_FAILURE}; + } + return vec; +} + +gxf::Expected CopyIntoDetection3D( + Detection3DListMessageParts message, PointerView /* class_ids */, + PointerView positions, PointerView quaternions, PointerView scores, + PointerView bbox_sizes, const std::string& object_name, const int32_t n_hypothesis) { + constexpr size_t kNumPointsPosition{3}; + constexpr size_t kNumPointsQuaternion{4}; + constexpr size_t kNumPointsBBoxSize{3}; + for (size_t i = 0; i < message.poses.size(); ++i) { + const size_t size_idx{i * kNumPointsBBoxSize}; + message.bbox_sizes[i].value()->x() = bbox_sizes.data[size_idx + 0]; + message.bbox_sizes[i].value()->y() = bbox_sizes.data[size_idx + 1]; + message.bbox_sizes[i].value()->z() = bbox_sizes.data[size_idx + 2]; + + const size_t position_idx{i * kNumPointsPosition}; + const size_t quaternion_idx{i * kNumPointsQuaternion}; + + *message.poses[i].value() = ::nvidia::isaac::Pose3d{ + ::nvidia::isaac::SO3d::FromQuaternion(::nvidia::isaac::Quaterniond{ + quaternions.data[quaternion_idx + 3], quaternions.data[quaternion_idx + 0], + quaternions.data[quaternion_idx + 1], quaternions.data[quaternion_idx + 2]}), + ::nvidia::isaac::Vector3d( + positions.data[position_idx + 0], positions.data[position_idx + 1], + positions.data[position_idx + 2])}; + + for (int32_t j = 0; j < n_hypothesis; ++j) { + message.hypothesis[i].value()->class_ids.push_back(object_name); + message.hypothesis[i].value()->scores.push_back(scores.data[i * n_hypothesis + j]); + } + } + return gxf::Success; +} + +gxf::Expected ToDetection3DList( + Detection3DListMessageParts message, gxf::Handle class_id_tensor, + gxf::Handle position_tensor, gxf::Handle quaternion_tensor, + gxf::Handle score_tensor, gxf::Handle bbox_size_tensor, + const std::string& object_name) { + PointerView class_ids_view; + PointerView positions_view; + PointerView quaternions_view; + PointerView scores_view; + PointerView bbox_sizes_view; + + // Ensure that the vectors don't go out of scope + std::vector class_ids; + std::vector positions, quaternions, scores, bbox_sizes; + + switch (class_id_tensor->storage_type()) { + case gxf::MemoryStorageType::kDevice: { + auto maybe_vector_result = ToVector(class_id_tensor) + .assign_to(class_ids) + .and_then([&]() { return ToVector(position_tensor); }) + .assign_to(positions) + .and_then([&]() { return ToVector(quaternion_tensor); }) + .assign_to(quaternions) + .and_then([&]() { return ToVector(score_tensor); }) + .assign_to(scores) + .and_then([&]() { return ToVector(bbox_size_tensor); }) + .assign_to(bbox_sizes); + if (!maybe_vector_result) { + GXF_LOG_ERROR("Failed to convert tensors into vectors!"); + return gxf::ForwardError(maybe_vector_result); + } + class_ids_view = PointerView{class_ids}; + positions_view = PointerView{positions}; + quaternions_view = PointerView{quaternions}; + scores_view = PointerView{scores}; + bbox_sizes_view = PointerView{bbox_sizes}; + + } break; + case gxf::MemoryStorageType::kHost: + case gxf::MemoryStorageType::kSystem: { + class_ids_view = PointerView{class_id_tensor}; + positions_view = PointerView{position_tensor}; + quaternions_view = PointerView{quaternion_tensor}; + scores_view = PointerView{score_tensor}; + bbox_sizes_view = PointerView{bbox_size_tensor}; + } break; + default: + return gxf::Unexpected{GXF_FAILURE}; + } + return CopyIntoDetection3D( + message, class_ids_view, positions_view, quaternions_view, scores_view, bbox_sizes_view, + object_name, score_tensor->shape().dimension(1)); +} + +} // namespace + +gxf_result_t CenterPoseDetectionToIsaac::registerInterface(gxf::Registrar* registrar) { + gxf::Expected result; + result &= registrar->parameter(input_, "input", "Input", "The input"); + result &= registrar->parameter(output_, "output", "Output", "The output"); + result &= registrar->parameter( + object_name_, "object_name", "Object Name", "The name of the object detected"); + return gxf::ToResultCode(result); +} + +gxf_result_t CenterPoseDetectionToIsaac::start() { + return GXF_SUCCESS; +} + +gxf_result_t CenterPoseDetectionToIsaac::tick() { + auto maybe_tensor_entity = input_->receive(); + if (!maybe_tensor_entity) { + GXF_LOG_ERROR("Failed to receive input message!"); + return gxf::ToResultCode(maybe_tensor_entity); + } + gxf::Entity tensor_entity = maybe_tensor_entity.value(); + gxf::Handle class_id_tensor, position_tensor, quaternion_tensor, score_tensor, + bbox_size_tensor; + gxf::Handle input_timestamp; + + auto getting_tensor_result = + tensor_entity.get("class_id") + .assign_to(class_id_tensor) + .and_then([&]() { return tensor_entity.get("position"); }) + .assign_to(position_tensor) + .and_then([&]() { return tensor_entity.get("quaternion_xyzw"); }) + .assign_to(quaternion_tensor) + .and_then([&]() { return tensor_entity.get("score"); }) + .assign_to(score_tensor) + .and_then([&]() { return tensor_entity.get("bbox_size"); }) + .assign_to(bbox_size_tensor) + .and_then([&]() { return tensor_entity.get("timestamp"); }) + .assign_to(input_timestamp); + if (!getting_tensor_result) { + GXF_LOG_ERROR("Failed to get all required tensors"); + return gxf::ToResultCode(getting_tensor_result); + } + auto maybe_detection3_d_list = + CreateDetection3DListMessage(context(), class_id_tensor->shape().dimension(0)); + if (!maybe_detection3_d_list) { + GXF_LOG_ERROR("Failed to create detection3d list"); + return gxf::ToResultCode(maybe_detection3_d_list); + } + + auto maybe_result = + ToDetection3DList( + maybe_detection3_d_list.value(), class_id_tensor, position_tensor, quaternion_tensor, + score_tensor, bbox_size_tensor, object_name_.get()) + .and_then([&]() { *maybe_detection3_d_list.value().timestamp = *input_timestamp; }); + if (!maybe_result) { + GXF_LOG_ERROR("Failed to transfer data into Isaac!"); + return gxf::ToResultCode(maybe_result); + } + + return gxf::ToResultCode(output_->publish(maybe_detection3_d_list.value().entity)); +} + +gxf_result_t CenterPoseDetectionToIsaac::stop() { + return GXF_SUCCESS; +} + +} // namespace centerpose +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_centerpose/gxf/centerpose/components/centerpose_detection_to_isaac.hpp b/isaac_ros_centerpose/gxf/centerpose/components/centerpose_detection_to_isaac.hpp new file mode 100644 index 0000000..3278e55 --- /dev/null +++ b/isaac_ros_centerpose/gxf/centerpose/components/centerpose_detection_to_isaac.hpp @@ -0,0 +1,47 @@ +// 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 "centerpose_detection.hpp" +#include "cuboid3d.hpp" +#include "cuboid_pnp_solver.hpp" +#include "gxf/std/allocator.hpp" +#include "gxf/std/codelet.hpp" +#include "gxf/std/parameter_parser_std.hpp" +#include "gxf/std/receiver.hpp" +#include "gxf/std/transmitter.hpp" + +namespace nvidia { +namespace isaac { +namespace centerpose { + +class CenterPoseDetectionToIsaac : public gxf::Codelet { + public: + gxf_result_t registerInterface(gxf::Registrar* registrar) override; + gxf_result_t start() override; + gxf_result_t tick() override; + gxf_result_t stop() override; + + private: + gxf::Parameter> input_; + gxf::Parameter> output_; + gxf::Parameter object_name_; +}; + +} // namespace centerpose +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_centerpose/gxf/centerpose/components/centerpose_postprocessor.cpp b/isaac_ros_centerpose/gxf/centerpose/components/centerpose_postprocessor.cpp new file mode 100644 index 0000000..4f4b908 --- /dev/null +++ b/isaac_ros_centerpose/gxf/centerpose/components/centerpose_postprocessor.cpp @@ -0,0 +1,533 @@ +// 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 "centerpose_postprocessor.hpp" +#include + +#include +#include +#include +#include +#include +#include + +#include "centerpose_detection.hpp" +#include "cuboid3d.hpp" +#include "cuboid_pnp_solver.hpp" +#include "cuda.h" +#include "cuda_runtime.h" +#include "Eigen/Dense" +#include "gxf/multimedia/camera.hpp" +#include "gxf/std/tensor.hpp" +#include "gxf/std/timestamp.hpp" +#include "opencv2/core/eigen.hpp" +#include "opencv2/opencv.hpp" +#include "soft_nms_nvidia.hpp" + +namespace nvidia { +namespace isaac { +namespace centerpose { + +namespace { + +constexpr std::array kTensorIdxToStr = { + "bboxes", "scores", "kps", "clses", "obj_scale", "kps_displacement_mean", "kps_heatmap_mean"}; + +const std::unordered_map kTensorStrToIdx = { + {"bboxes", 0}, {"scores", 1}, {"kps", 2}, + {"clses", 3}, {"obj_scale", 4}, {"kps_displacement_mean", 5}, + {"kps_heatmap_mean", 6}}; + +constexpr float kNMSNt{0.5f}; +constexpr float kNMSSigma{0.5f}; +const NMSMethod kNMSMethod{NMSMethod::GAUSSIAN}; + +constexpr char kTimestampName[] = "timestamp"; + +inline gxf::Expected>> GetInputTensors( + const gxf::Entity entity) { + std::vector> tensors; + tensors.reserve(kTensorIdxToStr.size()); + for (const auto& tensor_name : kTensorIdxToStr) { + auto maybe_tensor = entity.get(tensor_name); + if (!maybe_tensor) { + GXF_LOG_ERROR("Failed to get tensor element: %s", tensor_name); + return gxf::Unexpected{GXF_FAILURE}; + } + tensors.push_back(maybe_tensor.value()); + } + return tensors; +} + +Eigen::Matrix3fRM ComputeAffineTransform( + const Eigen::Vector2f& eigen_center, const float scale_scalar, const float rot, + const Eigen::Vector2i& output_field_size, bool inv = false) { + const cv::Point2f shift = {0.0f, 0.0f}; + + const float src_w{scale_scalar}; + const float dst_w{static_cast(output_field_size(0))}; + const float dst_h{static_cast(output_field_size(1))}; + + const float rot_rad{static_cast(M_PI * rot / 180.0f)}; + auto calculate_direction = [](const cv::Point2f& pt, const float rot_rad) { + return cv::Point2f{ + pt.x * std::cos(rot_rad) - pt.y * std::sin(rot_rad), + pt.x * std::sin(rot_rad) + pt.y * std::cos(rot_rad)}; + }; + const cv::Point2f src_direction = calculate_direction({0.0f, src_w * -0.5f}, rot_rad); + const cv::Point2f dst_direction = cv::Point2f{0.0f, dst_w * -0.5f}; + + const cv::Point2f center = {eigen_center(0), eigen_center(1)}; + + // Compute the points of interest in the original image + std::vector src_points; + src_points.push_back(center + scale_scalar * shift); + src_points.push_back(center + src_direction + scale_scalar * shift); + + auto calculate_third_point = [](const cv::Point2f& a, const cv::Point2f& b) { + const cv::Point2f direction = a - b; + return b + cv::Point2f{-direction.y, direction.x}; + }; + src_points.push_back(calculate_third_point(src_points[0], src_points[1])); + + // Compute the corresponding points of interest in the output_field_size image plane + std::vector dst_points; + dst_points.push_back({dst_w * 0.5f, dst_h * 0.5f}); + dst_points.push_back(cv::Point2f{dst_w * 0.5f, dst_h * 0.5f} + dst_direction); + dst_points.push_back(calculate_third_point(dst_points[0], dst_points[1])); + + // Use the computed src and dst points to find the affine transform (mapping + // b/w the two) + cv::Mat affine_matrix_cv = inv ? cv::getAffineTransform(dst_points, src_points) + : cv::getAffineTransform(src_points, dst_points); + Eigen::Matrix3dRM affine_matrix = Eigen::Matrix3dRM::Identity(); + cv::cv2eigen(affine_matrix_cv, affine_matrix); + + return affine_matrix.cast(); +} + +template +gxf::Expected ToTensor( + const std::vector>& eigen_type, + gxf::Handle tensor, gxf::Handle allocator, + const gxf::MemoryStorageType storage_type, const cudaMemcpyKind operation) { + // Handle special case when input is empty + if (eigen_type.size() == 0) { + return tensor->reshape(gxf::Shape{0, 0, 0}, storage_type, allocator); + } + return tensor + ->reshape( + gxf::Shape{ + static_cast(eigen_type.size()), static_cast(eigen_type[0].rows()), + static_cast(eigen_type[0].cols())}, + storage_type, allocator) + .and_then([&]() -> gxf::Expected { + for (size_t i = 0; i < eigen_type.size(); ++i) { + size_t size = tensor->size() / tensor->shape().dimension(0); + size_t start = i * size; + cudaError_t error = + cudaMemcpy(tensor->pointer() + start, eigen_type[i].data(), size, operation); + if (error != cudaSuccess) { + GXF_LOG_ERROR("Error while copying from device to host: %s", cudaGetErrorString(error)); + return gxf::Unexpected{GXF_FAILURE}; + } + } + return gxf::Success; + }); +} + +template +gxf::Expected ToTensor( + const std::vector& vec, gxf::Handle tensor, + gxf::Handle allocator, const gxf::MemoryStorageType storage_type, + const cudaMemcpyKind operation) { + return tensor->reshape(gxf::Shape{static_cast(vec.size()), 1}, storage_type, allocator) + .and_then([&]() -> gxf::Expected { + const cudaError_t error = + cudaMemcpy(tensor->pointer(), vec.data(), tensor->size(), operation); + if (error != cudaSuccess) { + GXF_LOG_ERROR("Error while copying from device to host: %s", cudaGetErrorString(error)); + return gxf::Unexpected{GXF_FAILURE}; + } + return gxf::Success; + }); +} + +inline Eigen::MatrixXfRM PerformAffineTransform( + const Eigen::Ref& untransformed_points, + const Eigen::Matrix3fRM& affine_transform) { + Eigen::MatrixXfRM transformed_points = + affine_transform.block<2, 2>(0, 0) * untransformed_points.transpose(); + transformed_points.colwise() += affine_transform.block<2, 1>(0, 2); + return transformed_points.transpose(); +} + +inline Eigen::MatrixXfRM Calculate2DKeypoints( + const Eigen::MatrixXfRM& kps_displacement_mean, const Eigen::Matrix3fRM& affine_transform) { + constexpr int32_t kps_displacement_rows{8}; + constexpr int32_t kps_displacement_cols{2}; + Eigen::Map reshaped_kps_displacement_mean{ + kps_displacement_mean.data(), kps_displacement_rows, kps_displacement_cols}; + return PerformAffineTransform(reshaped_kps_displacement_mean, affine_transform); +} + +inline Eigen::MatrixXfRM CalculateBBoxPoints( + const Eigen::MatrixXfRM& bbox, const Eigen::Matrix3fRM& affine_transform) { + constexpr int32_t bbox_rows{2}; + constexpr int32_t bbox_cols{2}; + Eigen::Map reshaped_bbox{bbox.data(), bbox_rows, bbox_cols}; + return PerformAffineTransform(reshaped_bbox, affine_transform); +} + +gxf::Expected SolvePnP( + const Eigen::MatrixXfRM& keypoints2d, const Eigen::MatrixXfRM& kps_heatmap_mean, + const Cuboid3d& cuboid3d, const Eigen::Matrix3f& camera_matrix) { + constexpr int32_t kps_heatmap_rows{8}; + constexpr int32_t kps_heatmap_cols{2}; + Eigen::Map reshaped_kps_heatmap_mean( + kps_heatmap_mean.data(), kps_heatmap_rows, kps_heatmap_cols); + Eigen::MatrixXfRM points_filtered( + keypoints2d.rows() + reshaped_kps_heatmap_mean.rows(), keypoints2d.cols()); + points_filtered << keypoints2d, reshaped_kps_heatmap_mean; + Eigen::Vector4f dist_coeffs{0.0f, 0.0f, 0.0f, 0.0f}; + + constexpr float pnp_scale_factor{1.0f}; + CuboidPnPSolver pnp_solver{pnp_scale_factor, camera_matrix, dist_coeffs, cuboid3d}; + auto maybe_result = pnp_solver.solvePnP(points_filtered); + if (!maybe_result) { + return gxf::Unexpected{GXF_FAILURE}; + } + return maybe_result.value(); +} + +Eigen::MatrixXfRM Calculate3DPoints(const PnPResult& pnp_result, const Cuboid3d& cuboid3d) { + Eigen::Matrix4f pose_pred = Eigen::Matrix4f::Identity(); + pose_pred.block<3, 3>(0, 0) = pnp_result.pose.orientation.normalized().toRotationMatrix(); + pose_pred.block<3, 1>(0, 3) = pnp_result.pose.position; + + const std::array< + Eigen::Vector3f, static_cast(CuboidVertexType::TOTAL_CORNER_VERTEX_COUNT)>& + points_3d_obj = cuboid3d.vertices(); + Eigen::MatrixXf points_3d_obj_stacked = + Eigen::MatrixXf(4, static_cast(CuboidVertexType::TOTAL_CORNER_VERTEX_COUNT)); + for (int i = 0; i < points_3d_obj_stacked.cols(); ++i) { + points_3d_obj_stacked.block<3, 1>(0, i) = points_3d_obj[i]; + points_3d_obj_stacked(3, i) = 1.0f; + } + Eigen::MatrixXfRM points_3d_cam = + (pose_pred * points_3d_obj_stacked).block(0, 0, 3, points_3d_obj_stacked.cols()).transpose(); + return points_3d_cam; +} + +} // namespace + +gxf_result_t CenterPosePostProcessor::registerInterface(gxf::Registrar* registrar) { + gxf::Expected result; + result &= registrar->parameter(input_, "input", "Input", "The input"); + result &= registrar->parameter(camera_model_input_, "camera_model_input"); + result &= registrar->parameter(output_, "output", "Output", "The output"); + result &= registrar->parameter(allocator_, "allocator", "Allocator", "The allocator"); + result &= registrar->parameter( + output_field_size_param_, "output_field_size", "Output Field Size", + "The size of the 2D keypoint decoding from the network output"); + result &= registrar->parameter( + cuboid_scaling_factor_, "cuboid_scaling_factor", "Cuboid Scaling Factor", + "Used to scale the cuboid used for calculating " + "the size of the objects detected", + 1.0f); + result &= registrar->parameter( + storage_type_, "storage type", "Storage Type", "Memory storage type of output frames.", + static_cast(gxf::MemoryStorageType::kHost)); + result &= registrar->parameter( + score_threshold_, "score_threshold", "Score Threshold", + "Any detections with scores less than this value will be discarded"); + return gxf::ToResultCode(result); +} + +gxf_result_t CenterPosePostProcessor::start() { + camera_matrix_ = Eigen::Matrix3f::Identity(); + original_image_size_ = Eigen::Vector2i{0, 0}; + output_field_size_ = + Eigen::Vector2i(output_field_size_param_.get()[0], output_field_size_param_.get()[1]); + return GXF_SUCCESS; +} + +gxf::Expected CenterPosePostProcessor::updateCameraProperties( + gxf::Handle camera_model) { + if (!camera_model) { + return gxf::Unexpected{GXF_FAILURE}; + } + + camera_matrix_(0, 0) = camera_model->focal_length.x; + camera_matrix_(0, 2) = camera_model->principal_point.x; + + camera_matrix_(1, 1) = camera_model->focal_length.y; + camera_matrix_(1, 2) = camera_model->principal_point.y; + + camera_matrix_(2, 2) = 1.0f; + + // Avoid re-computing affine transform if it's not necessary + if (original_image_size_.x() == static_cast(camera_model->dimensions.x) && + original_image_size_.y() == static_cast(camera_model->dimensions.y)) { + return gxf::Success; + } + + original_image_size_ = Eigen::Vector2i(camera_model->dimensions.x, camera_model->dimensions.y); + + const Eigen::Vector2f center = original_image_size_.cast() / 2.0f; + const float scale = std::max( + static_cast(camera_model->dimensions.x), + static_cast(camera_model->dimensions.y)); + + constexpr float rotation_deg{0.0f}; + constexpr bool inverse{true}; + affine_transform_ = + ComputeAffineTransform(center, scale, rotation_deg, output_field_size_, inverse); + return gxf::Success; +} + +gxf_result_t CenterPosePostProcessor::tick() { + auto maybe_camera_model_entity = camera_model_input_->receive(); + if (!maybe_camera_model_entity) { + GXF_LOG_ERROR("Failed to receive input camera model entity!"); + return gxf::ToResultCode(maybe_camera_model_entity); + } + + auto maybe_camera_model = maybe_camera_model_entity.value().get("intrinsics"); + if (!maybe_camera_model) { + GXF_LOG_ERROR("Failed to receive input camera model!"); + return gxf::ToResultCode(maybe_camera_model); + } + + auto maybe_updated_properties = updateCameraProperties(maybe_camera_model.value()); + if (!maybe_updated_properties) { + GXF_LOG_ERROR("Failed to update camera properties!"); + return gxf::ToResultCode(maybe_updated_properties); + } + + auto maybe_tensor_entity = input_->receive(); + if (!maybe_tensor_entity) { + GXF_LOG_ERROR("Failed to receive input message!"); + return gxf::ToResultCode(maybe_tensor_entity); + } + gxf::Entity tensor_entity = maybe_tensor_entity.value(); + + // Extract all tensors + auto maybe_tensors = GetInputTensors(tensor_entity); + if (!maybe_tensors) { + return gxf::ToResultCode(maybe_tensors); + } + std::vector> tensors = maybe_tensors.value(); + + // Assume all tensors share the same batch size + CenterPoseDetectionList detections; + for (int32_t batch = 0; batch < tensors[0]->shape().dimension(0); ++batch) { + std::vector batch_tensors; + // Transfer from device to host + for (size_t i = 0; i < tensors.size(); ++i) { + batch_tensors.push_back( + Eigen::MatrixXfRM(tensors[i]->shape().dimension(1), tensors[i]->shape().dimension(2))); + + size_t tensor_size = tensors[i]->size() / tensors[i]->shape().dimension(0); + size_t tensor_start_idx = batch * tensor_size; + const cudaError_t memcpy_error = cudaMemcpy( + batch_tensors[i].data(), tensors[i]->pointer() + tensor_start_idx, tensor_size, + cudaMemcpyDeviceToHost); + if (memcpy_error != cudaSuccess) { + GXF_LOG_ERROR("Error: %s", cudaGetErrorString(memcpy_error)); + return GXF_FAILURE; + } + } + CenterPoseDetectionList batch_detections = processTensor(batch_tensors); + detections.insert(detections.end(), batch_detections.begin(), batch_detections.end()); + } + + auto maybe_timestamp = tensor_entity.get(kTimestampName); + if (!maybe_timestamp) { + GXF_LOG_ERROR("Failed to get timestamp!"); + return gxf::ToResultCode(maybe_timestamp); + } + + return gxf::ToResultCode(publish(detections, maybe_timestamp.value())); +} + +CenterPoseDetectionList CenterPosePostProcessor::processTensor( + const std::vector& tensors) { + CenterPoseDetectionList detections; + for (int i = 0; i < tensors[kTensorStrToIdx.at("scores")].rows(); ++i) { + const float score{tensors[kTensorStrToIdx.at("scores")](i, 0)}; + const int cls{static_cast(tensors[kTensorStrToIdx.at("clses")](i, 0))}; + if (score < score_threshold_.get()) { + continue; + } + CenterPoseDetection detection; + detection.class_id = cls; + detection.score = score; + constexpr int32_t keypoints_size_flattened{16}; + constexpr int32_t bbox_size_flattened{4}; + constexpr int32_t kps_heatmap_size_flattened{16}; + constexpr int32_t obj_scale_size_flattened{3}; + detection.keypoints2d = Calculate2DKeypoints( + tensors[kTensorStrToIdx.at("kps_displacement_mean")].block<1, keypoints_size_flattened>( + i, 0), + affine_transform_); + detection.bbox = CalculateBBoxPoints( + tensors[kTensorStrToIdx.at("bboxes")].block<1, bbox_size_flattened>(i, 0), + affine_transform_); + detection.kps_heatmap_mean = + tensors[kTensorStrToIdx.at("kps_heatmap_mean")].block<1, kps_heatmap_size_flattened>(i, 0); + detection.bbox_size = + cuboid_scaling_factor_.get() * + tensors[kTensorStrToIdx.at("obj_scale")].block<1, obj_scale_size_flattened>(i, 0); + detections.push_back(detection); + } + + std::set indices = + SoftNMSNvidia(score_threshold_.get(), kNMSSigma, kNMSNt, kNMSMethod, &detections); + + CenterPoseDetectionList filtered_detections; + for (const size_t& idx : indices) { + filtered_detections.push_back(detections[idx]); + } + + for (CenterPoseDetection& detection : filtered_detections) { + Cuboid3d cuboid3d{detection.bbox_size}; + auto maybe_pnp_result = + SolvePnP(detection.keypoints2d, detection.kps_heatmap_mean, cuboid3d, camera_matrix_); + if (!maybe_pnp_result) { + continue; + } + PnPResult pnp_result = maybe_pnp_result.value(); + pnp_result.pose.orientation.normalize(); + Eigen::MatrixXfRM points_3d_cam = Calculate3DPoints(pnp_result, cuboid3d); + + Eigen::Vector3f points_3d_cam_mean = points_3d_cam.colwise().mean(); + Eigen::Vector2f projected_points_mean = pnp_result.projected_points.colwise().mean(); + + Eigen::MatrixXfRM keypoints3d(1 + points_3d_cam.rows(), points_3d_cam.cols()); + keypoints3d << points_3d_cam_mean.transpose(), points_3d_cam; + + Eigen::MatrixXfRM projected_keypoints2d( + 1 + pnp_result.projected_points.rows(), pnp_result.projected_points.cols()); + projected_keypoints2d << projected_points_mean.transpose(), pnp_result.projected_points; + detection.projected_keypoints_2d = projected_keypoints2d; + detection.keypoints3d = keypoints3d; + detection.position = pnp_result.pose.position; + detection.quaternion = pnp_result.pose.orientation; + } + + return filtered_detections; +} + +gxf::Expected CenterPosePostProcessor::publish( + const CenterPoseDetectionList& detections, gxf::Handle input_timestamp) { + std::vector class_id; + std::vector keypoints2d, projected_keypoints_2d, keypoints3d; + std::vector position; + std::vector quaternion; + std::vector score; + std::vector bbox_size; + + for (const auto& detection : detections) { + class_id.push_back(detection.class_id); + keypoints2d.push_back(detection.keypoints2d); + projected_keypoints_2d.push_back(detection.projected_keypoints_2d); + keypoints3d.push_back(detection.keypoints3d); + position.push_back(detection.position); + quaternion.push_back(Eigen::Vector4f{ + detection.quaternion.x(), detection.quaternion.y(), detection.quaternion.z(), + detection.quaternion.w()}); + score.push_back(detection.score); + bbox_size.push_back(detection.bbox_size); + } + + const gxf::MemoryStorageType storage_type{ + static_cast(storage_type_.get())}; + cudaMemcpyKind operation; + switch (storage_type) { + case gxf::MemoryStorageType::kDevice: { + operation = cudaMemcpyHostToDevice; + } break; + case gxf::MemoryStorageType::kHost: + case gxf::MemoryStorageType::kSystem: { + operation = cudaMemcpyHostToHost; + } break; + default: { + return gxf::Unexpected{GXF_PARAMETER_OUT_OF_RANGE}; + } break; + } + + gxf::Entity output_entity; + gxf::Handle class_id_tensor, keypoints2d_tensor, projected_keypoints_2d_tensor, + keypoints3d_tensor, position_tensor, quaternion_tensor, score_tensor, bbox_size_tensor; + gxf::Handle output_timestamp; + return gxf::Entity::New(context()) + .assign_to(output_entity) + .and_then([&]() { return output_entity.add("class_id"); }) + .assign_to(class_id_tensor) + .and_then([&]() { + return ToTensor(class_id, class_id_tensor, allocator_.get(), storage_type, operation); + }) + .and_then([&]() { return output_entity.add("keypoints2d"); }) + .assign_to(keypoints2d_tensor) + .and_then([&]() { + return ToTensor(keypoints2d, keypoints2d_tensor, allocator_.get(), storage_type, operation); + }) + .and_then([&]() { return output_entity.add("projected_keypoints2d"); }) + .assign_to(projected_keypoints_2d_tensor) + .and_then([&]() { + return ToTensor( + projected_keypoints_2d, projected_keypoints_2d_tensor, allocator_.get(), storage_type, + operation); + }) + .and_then([&]() { return output_entity.add("keypoints3d"); }) + .assign_to(keypoints3d_tensor) + .and_then([&]() { + return ToTensor(keypoints3d, keypoints3d_tensor, allocator_.get(), storage_type, operation); + }) + .and_then([&]() { return output_entity.add("position"); }) + .assign_to(position_tensor) + .and_then([&]() { + return ToTensor(position, position_tensor, allocator_.get(), storage_type, operation); + }) + .and_then([&]() { return output_entity.add("quaternion_xyzw"); }) + .assign_to(quaternion_tensor) + .and_then([&]() { + return ToTensor(quaternion, quaternion_tensor, allocator_.get(), storage_type, operation); + }) + .and_then([&]() { return output_entity.add("score"); }) + .assign_to(score_tensor) + .and_then([&]() { + return ToTensor(score, score_tensor, allocator_.get(), storage_type, operation); + }) + .and_then([&]() { return output_entity.add("bbox_size"); }) + .assign_to(bbox_size_tensor) + .and_then([&]() { + return ToTensor(bbox_size, bbox_size_tensor, allocator_.get(), storage_type, operation); + }) + .and_then([&]() { return output_entity.add(kTimestampName); }) + .assign_to(output_timestamp) + .and_then([&]() { *output_timestamp = *input_timestamp; }) + .and_then([&]() { return output_->publish(output_entity); }); +} + +gxf_result_t CenterPosePostProcessor::stop() { + return GXF_SUCCESS; +} + +} // namespace centerpose +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_centerpose/gxf/centerpose/components/centerpose_postprocessor.hpp b/isaac_ros_centerpose/gxf/centerpose/components/centerpose_postprocessor.hpp new file mode 100644 index 0000000..4b8f96f --- /dev/null +++ b/isaac_ros_centerpose/gxf/centerpose/components/centerpose_postprocessor.hpp @@ -0,0 +1,69 @@ +// 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 "centerpose_detection.hpp" +#include "centerpose_types.hpp" +#include "cuboid3d.hpp" +#include "cuboid_pnp_solver.hpp" +#include "Eigen/Dense" +#include "gxf/multimedia/camera.hpp" +#include "gxf/std/allocator.hpp" +#include "gxf/std/codelet.hpp" +#include "gxf/std/parameter_parser_std.hpp" +#include "gxf/std/receiver.hpp" +#include "gxf/std/timestamp.hpp" +#include "gxf/std/transmitter.hpp" + +namespace nvidia { +namespace isaac { +namespace centerpose { + +class CenterPosePostProcessor : public gxf::Codelet { + public: + gxf_result_t registerInterface(gxf::Registrar* registrar) override; + gxf_result_t start() override; + gxf_result_t tick() override; + gxf_result_t stop() override; + + private: + CenterPoseDetectionList processTensor(const std::vector& tensors); + + gxf::Expected publish( + const CenterPoseDetectionList& detections, gxf::Handle input_timestamp); + + gxf::Expected updateCameraProperties(gxf::Handle camera_model); + + private: + gxf::Parameter> input_; + gxf::Parameter> camera_model_input_; + gxf::Parameter> output_; + gxf::Parameter> allocator_; + gxf::Parameter> output_field_size_param_; + gxf::Parameter cuboid_scaling_factor_; + gxf::Parameter storage_type_; + gxf::Parameter score_threshold_; + + Eigen::Matrix3f camera_matrix_; + Eigen::Vector2i original_image_size_; + Eigen::Vector2i output_field_size_; + Eigen::Matrix3fRM affine_transform_; +}; + +} // namespace centerpose +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_centerpose/gxf/centerpose/components/centerpose_types.hpp b/isaac_ros_centerpose/gxf/centerpose/components/centerpose_types.hpp new file mode 100644 index 0000000..f34c98d --- /dev/null +++ b/isaac_ros_centerpose/gxf/centerpose/components/centerpose_types.hpp @@ -0,0 +1,27 @@ +// 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 "Eigen/Dense" + +namespace Eigen { + +using MatrixXfRM = Eigen::Matrix; +using Matrix3fRM = Eigen::Matrix; +using Matrix3dRM = Eigen::Matrix; + +} // namespace Eigen diff --git a/isaac_ros_centerpose/gxf/centerpose/components/centerpose_visualizer.cpp b/isaac_ros_centerpose/gxf/centerpose/components/centerpose_visualizer.cpp new file mode 100644 index 0000000..14a6641 --- /dev/null +++ b/isaac_ros_centerpose/gxf/centerpose/components/centerpose_visualizer.cpp @@ -0,0 +1,304 @@ +// 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 "centerpose_visualizer.hpp" + +#include "centerpose_types.hpp" +#include "cuboid3d.hpp" +#include "cuda.h" +#include "cuda_runtime.h" +#include "detection3_d_array_message.hpp" +#include "Eigen/Dense" +#include "gxf/multimedia/camera.hpp" +#include "gxf/multimedia/video.hpp" +#include "opencv2/core/eigen.hpp" +#include "opencv2/opencv.hpp" +#include "video_buffer_utils.hpp" + +namespace nvidia { +namespace isaac { +namespace centerpose { + +namespace { + +gxf::Expected CopyVideoBuffer( + gxf::Handle output_video_buffer, + gxf::Handle input_video_buffer) { + cudaMemcpyKind operation; + switch (input_video_buffer->storage_type()) { + case gxf::MemoryStorageType::kDevice: { + operation = cudaMemcpyDeviceToHost; + } break; + case gxf::MemoryStorageType::kHost: + case gxf::MemoryStorageType::kSystem: { + operation = cudaMemcpyHostToHost; + } break; + default: { + return gxf::Unexpected{GXF_PARAMETER_OUT_OF_RANGE}; + } break; + } + + cudaError_t error = cudaMemcpy( + output_video_buffer->pointer(), input_video_buffer->pointer(), input_video_buffer->size(), + operation); + if (error != cudaSuccess) { + GXF_LOG_ERROR("%s: %s", cudaGetErrorName(error), cudaGetErrorString(error)); + return gxf::Unexpected{GXF_FAILURE}; + } + return gxf::Success; +} + +Eigen::MatrixXfRM Calculate3DPoints(const Eigen::Matrix4f& pose_pred, const Cuboid3d& cuboid3d) { + const std::array< + Eigen::Vector3f, static_cast(CuboidVertexType::TOTAL_CORNER_VERTEX_COUNT)>& + points_3d_obj = cuboid3d.vertices(); + Eigen::MatrixXf points_3d_obj_stacked = + Eigen::MatrixXf(4, static_cast(CuboidVertexType::TOTAL_CORNER_VERTEX_COUNT)); + for (int i = 0; i < points_3d_obj_stacked.cols(); ++i) { + points_3d_obj_stacked.block<3, 1>(0, i) = points_3d_obj[i]; + points_3d_obj_stacked(3, i) = 1.0f; + } + Eigen::MatrixXfRM points_3d_cam = + (pose_pred * points_3d_obj_stacked).block(0, 0, 3, points_3d_obj_stacked.cols()).transpose(); + return points_3d_cam; +} + +void DarkenROI(const std::vector& reprojected_points, cv::Mat& img) { + cv::Mat dark_layer = img.clone(); + + std::vector polys; + for (const auto& point : reprojected_points) { + polys.push_back(point); + } + + std::vector convex_hull(polys.size()); + cv::convexHull(polys, convex_hull); + cv::fillConvexPoly(dark_layer, convex_hull, cv::Scalar{0.0, 0.0, 0.0}); + + constexpr double kAlpha{0.7}; + cv::addWeighted(img, kAlpha, dark_layer, 1.0 - kAlpha, 0, img); +} + +void DrawBoundingBox( + const std::vector& reprojected_points, const int32_t color_int, cv::Mat& img) { + const std::vector edges = {cv::Point2i{2, 4}, cv::Point2i{2, 6}, cv::Point2i{6, 8}, + cv::Point2i{4, 8}, cv::Point2i{1, 2}, cv::Point2i{3, 4}, + cv::Point2i{5, 6}, cv::Point2i{7, 8}, cv::Point2i{1, 3}, + cv::Point2i{1, 5}, cv::Point2i{3, 7}, cv::Point2i{5, 7}}; + + cv::Scalar color = { + static_cast((color_int >> 16) & 0xFF), static_cast((color_int >> 8) & 0xFF), + static_cast(color_int & 0xFF)}; + for (const cv::Point2i& edge : edges) { + const int start_idx{edge.x - 1}; + const int end_idx{edge.y - 1}; + cv::line(img, reprojected_points[start_idx], reprojected_points[end_idx], color, 2); + } +} + +void DrawAxes( + const Eigen::MatrixXfRM& keypoints3d, const Eigen::Matrix3f& camera_matrix, cv::Mat& img) { + const std::vector axes_point_list = { + Eigen::Vector3f{0.0f, 0.0f, 0.0f}, + keypoints3d.block<1, 3>(3, 0) - keypoints3d.block<1, 3>(1, 0), + keypoints3d.block<1, 3>(2, 0) - keypoints3d.block<1, 3>(1, 0), + keypoints3d.block<1, 3>(5, 0) - keypoints3d.block<1, 3>(1, 0), + }; + + std::vector viewport_points; + for (const auto& axes_point : axes_point_list) { + Eigen::Vector3f vector = axes_point.norm() == 0.0f ? Eigen::Vector3f{0.0f, 0.0f, 0.0f} + : axes_point / axes_point.norm() * 0.5f; + vector += keypoints3d.block<1, 3>(0, 0); + Eigen::Vector3f pp = camera_matrix * vector; + if (pp.z() != 0.0f) { + pp.x() = pp.x() / pp.z(); + pp.y() = pp.y() / pp.z(); + } + viewport_points.push_back(cv::Point2i{static_cast(pp.x()), static_cast(pp.y())}); + } + + const std::array colors = { + cv::Scalar{0, 255, 0}, cv::Scalar{255, 0, 0}, cv::Scalar{0, 0, 255}}; + + for (size_t i = 0; i < colors.size(); ++i) { + cv::line(img, viewport_points[0], viewport_points[i + 1], colors[i], 5); + } +} + +gxf::Expected DrawDetections( + Detection3DListMessageParts detections, gxf::Handle output_video_buffer, + Eigen::Matrix3f camera_matrix_eigen, const bool show_axes, const int32_t bounding_box_color) { + cv::Mat img{ + static_cast(output_video_buffer->video_frame_info().height), + static_cast(output_video_buffer->video_frame_info().width), CV_8UC3, + output_video_buffer->pointer()}; + cv::Mat camera_matrix_cv; + cv::eigen2cv(camera_matrix_eigen, camera_matrix_cv); + for (size_t i = 0; i < detections.count; ++i) { + gxf::Handle pose = detections.poses.at(i).value(); + gxf::Handle bbox_size = detections.bbox_sizes.at(i).value(); + + Cuboid3d cuboid3d{*bbox_size}; + + Eigen::MatrixXfRM points_3d_cam = Calculate3DPoints(pose->matrix().cast(), cuboid3d); + + Eigen::Matrix3f rot_matrix_eigen = pose->rotation.matrix().cast(); + cv::Mat rot_matrix; + cv::eigen2cv(rot_matrix_eigen, rot_matrix); + + // Note: points3d is already given in camera frame + cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64F); + cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64F); + cv::Mat dist_coeffs = cv::Mat::zeros(4, 1, CV_64F); + std::vector reprojected_points; + + cv::Mat points_3d_cam_cv; + cv::eigen2cv(points_3d_cam, points_3d_cam_cv); + cv::projectPoints( + points_3d_cam_cv, rvec, tvec, camera_matrix_cv, dist_coeffs, reprojected_points); + + DarkenROI(reprojected_points, img); + DrawBoundingBox(reprojected_points, bounding_box_color, img); + + if (show_axes) { + Eigen::Vector3f points_3d_cam_mean = points_3d_cam.colwise().mean(); + Eigen::MatrixXfRM keypoints3d(1 + points_3d_cam.rows(), points_3d_cam.cols()); + keypoints3d << points_3d_cam_mean.transpose(), points_3d_cam; + DrawAxes(keypoints3d, camera_matrix_eigen, img); + } + } + return gxf::Success; +} + +gxf::Expected GetCameraMatrix(gxf::Handle camera_model) { + if (!camera_model) { + return gxf::Unexpected{GXF_FAILURE}; + } + + Eigen::Matrix3f camera_matrix = Eigen::Matrix3f::Identity(); + camera_matrix(0, 0) = camera_model->focal_length.x; + camera_matrix(0, 2) = camera_model->principal_point.x; + + camera_matrix(1, 1) = camera_model->focal_length.y; + camera_matrix(1, 2) = camera_model->principal_point.y; + + camera_matrix(2, 2) = 1.0f; + return camera_matrix; +} + +} // namespace + +gxf_result_t CenterPoseVisualizer::registerInterface(gxf::Registrar* registrar) { + gxf::Expected result; + result &= registrar->parameter(video_buffer_input_, "video_buffer_input"); + result &= registrar->parameter(detections_input_, "detections_input"); + result &= registrar->parameter(camera_model_input_, "camera_model_input"); + result &= registrar->parameter(output_, "output"); + result &= registrar->parameter(allocator_, "allocator"); + result &= registrar->parameter(show_axes_, "show_axes"); + result &= registrar->parameter(bounding_box_color_, "bounding_box_color"); + return gxf::ToResultCode(result); +} + +gxf_result_t CenterPoseVisualizer::start() { + return GXF_SUCCESS; +} + +gxf_result_t CenterPoseVisualizer::tick() { + gxf::Entity video_buffer_input_entity, detections_input_entity, camera_model_input_entity; + gxf::Handle input_video_buffer; + Detection3DListMessageParts detections; + gxf::Handle camera_model; + + Eigen::Matrix3f camera_matrix; + auto maybe_received_inputs = + video_buffer_input_->receive() + .assign_to(video_buffer_input_entity) + .and_then([&]() { return video_buffer_input_entity.get(); }) + .assign_to(input_video_buffer) + .and_then([&]() { return detections_input_->receive(); }) + .assign_to(detections_input_entity) + .and_then([&]() { return GetDetection3DListMessage(detections_input_entity); }) + .assign_to(detections) + .and_then([&]() { return camera_model_input_->receive(); }) + .assign_to(camera_model_input_entity) + .and_then([&]() { return camera_model_input_entity.get("intrinsics"); }) + .assign_to(camera_model) + .and_then([&]() { return GetCameraMatrix(camera_model); }) + .assign_to(camera_matrix); + if (!maybe_received_inputs) { + GXF_LOG_ERROR("Failed to get all required inputs!"); + return gxf::ToResultCode(maybe_received_inputs); + } + + gxf::Entity output_entity; + gxf::Handle output_video_buffer; + auto maybe_created_outputs = + gxf::Entity::New(context()) + .assign_to(output_entity) + .and_then([&]() { return output_entity.add(); }) + .assign_to(output_video_buffer) + .and_then([&]() -> gxf::Expected { + switch (input_video_buffer->video_frame_info().color_format) { + case gxf::VideoFormat::GXF_VIDEO_FORMAT_RGB: + return AllocateVideoBuffer( + output_video_buffer, input_video_buffer->video_frame_info().width, + input_video_buffer->video_frame_info().height, gxf::MemoryStorageType::kHost, + allocator_.get()); + case gxf::VideoFormat::GXF_VIDEO_FORMAT_BGR: + return AllocateVideoBuffer( + output_video_buffer, input_video_buffer->video_frame_info().width, + input_video_buffer->video_frame_info().height, gxf::MemoryStorageType::kHost, + allocator_.get()); + default: { + GXF_LOG_ERROR("Received unsupported color format!"); + return gxf::Unexpected{GXF_FAILURE}; + } + } + }); + if (!maybe_created_outputs) { + GXF_LOG_ERROR("Failed to create output video buffer!"); + return gxf::ToResultCode(maybe_created_outputs); + } + + auto maybe_copied = CopyVideoBuffer(output_video_buffer, input_video_buffer); + if (!maybe_copied) { + return gxf::ToResultCode(maybe_copied); + } + DrawDetections( + detections, output_video_buffer, camera_matrix, show_axes_.get(), bounding_box_color_.get()); + + gxf::Handle timestamp; + auto maybe_added_timestamp = + output_entity.add("timestamp").assign_to(timestamp).and_then([&]() { + *timestamp = *detections.timestamp; + }); + if (!maybe_added_timestamp) { + GXF_LOG_ERROR("Could not add timestamp!"); + return gxf::ToResultCode(maybe_added_timestamp); + } + + return gxf::ToResultCode(output_->publish(output_entity)); +} + +gxf_result_t CenterPoseVisualizer::stop() { + return GXF_SUCCESS; +} + +} // namespace centerpose +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_centerpose/gxf/centerpose/components/centerpose_visualizer.hpp b/isaac_ros_centerpose/gxf/centerpose/components/centerpose_visualizer.hpp new file mode 100644 index 0000000..5d7b50d --- /dev/null +++ b/isaac_ros_centerpose/gxf/centerpose/components/centerpose_visualizer.hpp @@ -0,0 +1,51 @@ +// 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 "Eigen/Dense" +#include "gxf/std/allocator.hpp" +#include "gxf/std/codelet.hpp" +#include "gxf/std/parameter_parser_std.hpp" +#include "gxf/std/receiver.hpp" +#include "gxf/std/timestamp.hpp" +#include "gxf/std/transmitter.hpp" + +namespace nvidia { +namespace isaac { +namespace centerpose { + +class CenterPoseVisualizer : public gxf::Codelet { + public: + gxf_result_t registerInterface(gxf::Registrar* registrar) override; + gxf_result_t start() override; + gxf_result_t tick() override; + gxf_result_t stop() override; + + private: + gxf::Parameter> video_buffer_input_; + gxf::Parameter> detections_input_; + gxf::Parameter> camera_model_input_; + gxf::Parameter> output_; + gxf::Parameter> allocator_; + + gxf::Parameter show_axes_; + gxf::Parameter bounding_box_color_; +}; + +} // namespace centerpose +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_centerpose/gxf/centerpose/components/cuboid3d.cpp b/isaac_ros_centerpose/gxf/centerpose/components/cuboid3d.cpp new file mode 100644 index 0000000..e9d25bd --- /dev/null +++ b/isaac_ros_centerpose/gxf/centerpose/components/cuboid3d.cpp @@ -0,0 +1,76 @@ +// 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 "cuboid3d.hpp" +#include "centerpose_types.hpp" + +namespace nvidia { +namespace isaac { +namespace centerpose { + +namespace { + +std::array(CuboidVertexType::TOTAL_CORNER_VERTEX_COUNT)> +GenerateVertices(const Eigen::Vector3f& center_location, const Eigen::Vector3f& size_3d) { + const float width{size_3d[0]}; + const float height{size_3d[1]}; + const float depth{size_3d[2]}; + + const float cx{center_location[0]}; + const float cy{center_location[1]}; + const float cz{center_location[2]}; + + const float right{cx + width / 2.0f}; + const float left{cx - width / 2.0f}; + + const float top{cy + height / 2.0f}; + const float bottom{cy - height / 2.0f}; + + const float front{cz + depth / 2.0f}; + const float rear{cz - depth / 2.0f}; + + return {Eigen::Vector3f{left, bottom, rear}, Eigen::Vector3f{left, bottom, front}, + Eigen::Vector3f{left, top, rear}, Eigen::Vector3f{left, top, front}, + + Eigen::Vector3f{right, bottom, rear}, Eigen::Vector3f{right, bottom, front}, + Eigen::Vector3f{right, top, rear}, Eigen::Vector3f{right, top, front}}; +} +} // namespace + +Cuboid3d::Cuboid3d() + : center_location_{0.0f, 0.0f, 0.0f}, + size_3d_{1.0f, 1.0f, 1.0f}, + vertices_{GenerateVertices(center_location_, size_3d_)} {} + +Cuboid3d::Cuboid3d(const Eigen::Vector3f& size_3d) + : center_location_{0.0f, 0.0f, 0.0f}, + size_3d_{size_3d}, + vertices_{GenerateVertices(center_location_, size_3d)} {} + +const std::array(CuboidVertexType::TOTAL_CORNER_VERTEX_COUNT)>& +Cuboid3d::vertices() const { + return vertices_; +} + +std::array(CuboidVertexType::TOTAL_CORNER_VERTEX_COUNT)> +Cuboid3d::vertices() { + return vertices_; +} + +} // namespace centerpose +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_centerpose/gxf/centerpose/components/cuboid3d.hpp b/isaac_ros_centerpose/gxf/centerpose/components/cuboid3d.hpp new file mode 100644 index 0000000..3b59e16 --- /dev/null +++ b/isaac_ros_centerpose/gxf/centerpose/components/cuboid3d.hpp @@ -0,0 +1,65 @@ +// 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 "Eigen/Dense" + +namespace nvidia { +namespace isaac { +namespace centerpose { + +enum class CuboidVertexType : size_t { + FRONT_TOP_RIGHT = 0, + FRONT_TOP_LEFT = 1, + FRONT_BOTTOM_LEFT = 2, + FRONT_BOTTOM_RIGHT = 3, + REAR_TOP_RIGHT = 4, + REAR_TOP_LEFT = 5, + REAR_BOTTOM_LEFT = 6, + REAR_BOTTOM_RIGHT = 7, + CENTER = 8, + TOTAL_CORNER_VERTEX_COUNT = 8, // Corner vertices don't include the center + // point + TOTAL_VERTEX_COUNT = 9 +}; + +class Cuboid3d { + public: + Cuboid3d(); + + explicit Cuboid3d(const Eigen::Vector3f& size_3d); + + const std::array< + Eigen::Vector3f, static_cast(CuboidVertexType::TOTAL_CORNER_VERTEX_COUNT)>& + vertices() const; + + std::array(CuboidVertexType::TOTAL_CORNER_VERTEX_COUNT)> + vertices(); + + private: + Eigen::Vector3f center_location_; + Eigen::Vector3f size_3d_; + + std::array(CuboidVertexType::TOTAL_CORNER_VERTEX_COUNT)> + vertices_; +}; + +} // namespace centerpose +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_centerpose/gxf/centerpose/components/cuboid_pnp_solver.cpp b/isaac_ros_centerpose/gxf/centerpose/components/cuboid_pnp_solver.cpp new file mode 100644 index 0000000..69823f0 --- /dev/null +++ b/isaac_ros_centerpose/gxf/centerpose/components/cuboid_pnp_solver.cpp @@ -0,0 +1,104 @@ +// 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 "cuboid_pnp_solver.hpp" + +#include + +#include "opencv2/core/eigen.hpp" +#include "opencv2/opencv.hpp" + +namespace nvidia { +namespace isaac { +namespace centerpose { +namespace { + +Eigen::Quaternionf ConvertRvecToQuaternion(cv::Mat rvec_cv) { + Eigen::Vector3f rvec = Eigen::Vector3f{ + static_cast(rvec_cv.at(0, 0)), static_cast(rvec_cv.at(0, 1)), + static_cast(rvec_cv.at(0, 2))}; + const float theta{rvec.norm()}; + Eigen::Vector3f raxis = rvec / theta; + return Eigen::Quaternionf{Eigen::AngleAxisf(theta, raxis)}; +} + +constexpr float kInvalidPoint{-5000}; +constexpr int32_t kXIndex{0}; +constexpr int32_t kYIndex{1}; +constexpr int32_t kZIndex{2}; + +} // namespace + +std::optional CuboidPnPSolver::solvePnP( + const Eigen::MatrixXfRM& cuboid2d_points, const int pnp_algorithm) { + // Most of this code is just converting from Eigen to opencv + const std::array< + Eigen::Vector3f, static_cast(CuboidVertexType::TOTAL_CORNER_VERTEX_COUNT)>& + cuboid3d_points = cuboid3d_.vertices(); + std::vector obj_2d_points; + std::vector obj_3d_points; + // Filter out any invalid readings + for (size_t i = 0; i < static_cast(CuboidVertexType::TOTAL_CORNER_VERTEX_COUNT); ++i) { + Eigen::Vector2f point_2d = cuboid2d_points.block<1, 2>(i, 0); + if (point_2d.x() < kInvalidPoint || point_2d.y() < kInvalidPoint) { + continue; + } + obj_2d_points.push_back(cv::Point2f{point_2d.x(), point_2d.y()}); + obj_3d_points.push_back( + cv::Point3f{cuboid3d_points[i].x(), cuboid3d_points[i].y(), cuboid3d_points[i].z()}); + } + + if (obj_2d_points.size() < min_required_points_) { + return std::nullopt; + } + + // Convert from Eigen -> cv + cv::Mat camera_matrix; + cv::eigen2cv(camera_matrix_, camera_matrix); + + cv::Mat dist_coeffs; + cv::eigen2cv(dist_coeffs_, dist_coeffs); + + cv::Mat rvec, tvec; + bool ret = cv::solvePnP( + obj_3d_points, obj_2d_points, camera_matrix, dist_coeffs, rvec, tvec, false, pnp_algorithm); + if (!ret) { + return std::nullopt; + } + + std::vector reprojected_points; + cv::projectPoints(obj_3d_points, rvec, tvec, camera_matrix, dist_coeffs, reprojected_points); + + // Convert back to Eigen types + PnPResult result; + result.pose.position.x() = static_cast(tvec.at(0, kXIndex)); + result.pose.position.y() = static_cast(tvec.at(0, kYIndex)); + result.pose.position.z() = static_cast(tvec.at(0, kZIndex)); + + result.pose.orientation = ConvertRvecToQuaternion(rvec); + + result.projected_points = Eigen::MatrixXfRM(reprojected_points.size(), 2); + for (size_t i = 0; i < reprojected_points.size(); ++i) { + result.projected_points(i, kXIndex) = static_cast(reprojected_points[i].x); + result.projected_points(i, kYIndex) = static_cast(reprojected_points[i].y); + } + + return result; +} + +} // namespace centerpose +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_centerpose/gxf/centerpose/components/cuboid_pnp_solver.hpp b/isaac_ros_centerpose/gxf/centerpose/components/cuboid_pnp_solver.hpp new file mode 100644 index 0000000..5f114de --- /dev/null +++ b/isaac_ros_centerpose/gxf/centerpose/components/cuboid_pnp_solver.hpp @@ -0,0 +1,102 @@ +// 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 "centerpose_types.hpp" +#include "cuboid3d.hpp" +#include "Eigen/Dense" + +namespace nvidia { +namespace isaac { +namespace centerpose { + +struct PnPResult { + struct Pose { + Eigen::Vector3f position{}; + Eigen::Quaternionf orientation{}; + }; + + Pose pose; + + Eigen::MatrixXfRM projected_points{}; + float reprojection_error{}; +}; + +class CuboidPnPSolver { + public: + explicit CuboidPnPSolver(const Cuboid3d& cuboid3d) + : scaling_factor_{1.0f}, + camera_matrix_{Eigen::Matrix3f::Zero()}, + cuboid3d_{cuboid3d}, + dist_coeffs_{0.0f, 0.0f, 0.0f, 0.0f}, + min_required_points_{4} {} + + CuboidPnPSolver(const float scaling_factor, const Cuboid3d& cuboid3d) + : scaling_factor_{scaling_factor}, + camera_matrix_{Eigen::Matrix3f::Zero()}, + cuboid3d_{cuboid3d}, + dist_coeffs_{0.0f, 0.0f, 0.0f, 0.0f}, + min_required_points_{4} {} + + CuboidPnPSolver( + const float scaling_factor, const Eigen::Matrix3f& camera_matrix, const Cuboid3d& cuboid3d) + : scaling_factor_{scaling_factor}, + camera_matrix_{camera_matrix}, + cuboid3d_{cuboid3d}, + dist_coeffs_{0.0f, 0.0f, 0.0f, 0.0f}, + min_required_points_{4} {} + + CuboidPnPSolver( + const float scaling_factor, const Eigen::Matrix3f& camera_matrix, + const Eigen::Vector4f& dist_coeffs, const Cuboid3d& cuboid3d) + : scaling_factor_{scaling_factor}, + camera_matrix_{camera_matrix}, + cuboid3d_{cuboid3d}, + dist_coeffs_{dist_coeffs}, + min_required_points_{4} {} + + CuboidPnPSolver( + const float scaling_factor, const Eigen::Matrix3f& camera_matrix, + const Eigen::Vector4f& dist_coeffs, const Cuboid3d& cuboid3d, size_t min_required_points) + : scaling_factor_{scaling_factor}, + camera_matrix_{camera_matrix}, + cuboid3d_{cuboid3d}, + dist_coeffs_{dist_coeffs}, + min_required_points_{min_required_points} {} + + void setCameraMatrix(const Eigen::Matrix3f& camera_matrix) { camera_matrix_ = camera_matrix; } + + void setDistCoeffs(const Eigen::Vector4f& dist_coeffs) { dist_coeffs_ = dist_coeffs; } + + // 0 corresponds to SOLVEPNP_ITERATIVE + std::optional solvePnP( + const Eigen::MatrixXfRM& cuboid2d_points, const int pnp_algorithm = 0); + + private: + float scaling_factor_{}; + Eigen::Matrix3f camera_matrix_; + Cuboid3d cuboid3d_; + Eigen::Vector4f dist_coeffs_; + size_t min_required_points_; +}; + +} // namespace centerpose +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_centerpose/gxf/centerpose/components/soft_nms_nvidia.cpp b/isaac_ros_centerpose/gxf/centerpose/components/soft_nms_nvidia.cpp new file mode 100644 index 0000000..597091e --- /dev/null +++ b/isaac_ros_centerpose/gxf/centerpose/components/soft_nms_nvidia.cpp @@ -0,0 +1,139 @@ +// 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 "soft_nms_nvidia.hpp" + +#include +#include +#include +#include +#include +#include + +#include "centerpose_detection.hpp" + +namespace nvidia { +namespace isaac { +namespace centerpose { + +namespace { + +size_t SearchForMaxScoreIndex(const CenterPoseDetectionList* detections, const size_t start_idx) { + size_t max_idx{start_idx}; + float max_score{(*detections)[max_idx].score}; + for (size_t i = start_idx + 1; i < detections->size(); ++i) { + if (max_score < (*detections)[i].score) { + max_score = (*detections)[i].score; + max_idx = i; + } + } + return max_idx; +} + +float ComputeNMSWeight(float ov, float sigma, float Nt, NMSMethod method) { + switch (method) { + case NMSMethod::LINEAR: + return (ov > Nt) ? 1.0f - ov : 1.0f; + case NMSMethod::GAUSSIAN: + return std::exp(-(ov * ov) / sigma); + case NMSMethod::ORIGINAL: + return (ov > Nt) ? 0.0f : 1.0f; + } + return 1.0f; +} + +std::vector DoNMSIterations( + const size_t start_idx, const float threshold, const float sigma, const float Nt, + const NMSMethod method, size_t* total_detections, CenterPoseDetectionList* detections) { + float tx1 = (*detections)[start_idx].bbox(0, 0); + float ty1 = (*detections)[start_idx].bbox(0, 1); + float tx2 = (*detections)[start_idx].bbox(1, 0); + float ty2 = (*detections)[start_idx].bbox(1, 1); + + size_t curr_idx{start_idx + 1}; + + std::vector remove_indices; + while (curr_idx < *total_detections) { + float x1 = (*detections)[curr_idx].bbox(0, 0); + float y1 = (*detections)[curr_idx].bbox(0, 1); + float x2 = (*detections)[curr_idx].bbox(1, 0); + float y2 = (*detections)[curr_idx].bbox(1, 1); + float area = (x2 - x1 + 1) * (y2 - y1 + 1); + float iw = std::min(tx2, x2) - std::max(tx1, x1) + 1; + if (iw < 0) { + continue; + } + float ih = std::min(ty2, y2) - std::max(ty1, y1) + 1; + if (ih < 0) { + continue; + } + float ua = static_cast((tx2 - tx1 + 1) * (ty2 - ty1 + 1) + area - iw * ih); + float ov = iw * ih / ua; // iou between max box and detection box + float weight = ComputeNMSWeight(ov, sigma, Nt, method); + (*detections)[curr_idx].score = weight * (*detections)[curr_idx].score; + if ((*detections)[curr_idx].score < threshold) { + remove_indices.push_back(curr_idx); + *total_detections = *total_detections - 1; + curr_idx--; + } + curr_idx++; + } + return remove_indices; +} + +} // namespace + +std::set SoftNMSNvidia( + const float threshold, const float sigma, const float Nt, const NMSMethod method, + CenterPoseDetectionList* src_detections) { + if (src_detections->empty()) { + return {}; + } + + if (src_detections->size() == 1) { + return {0}; + } + + size_t N = src_detections->size(); + std::set total_remove_indices; + + for (size_t i = 0; i < N; ++i) { + size_t max_idx{SearchForMaxScoreIndex(src_detections, i)}; + std::swap((*src_detections)[i], (*src_detections)[max_idx]); + std::vector remove_indices = + DoNMSIterations(i, threshold, sigma, Nt, method, &N, src_detections); + for (const size_t idx : remove_indices) { + total_remove_indices.insert(total_remove_indices.end(), idx); + } + } + + std::set all_indices; + for (size_t i = 0; i < src_detections->size(); ++i) { + all_indices.insert(all_indices.end(), i); + } + + std::set keep_indices; + std::set_difference( + all_indices.begin(), all_indices.end(), total_remove_indices.begin(), + total_remove_indices.end(), std::inserter(keep_indices, keep_indices.end())); + return keep_indices; +} + +} // namespace centerpose + +} // namespace isaac + +} // namespace nvidia diff --git a/isaac_ros_centerpose/gxf/centerpose/components/soft_nms_nvidia.hpp b/isaac_ros_centerpose/gxf/centerpose/components/soft_nms_nvidia.hpp new file mode 100644 index 0000000..514ca10 --- /dev/null +++ b/isaac_ros_centerpose/gxf/centerpose/components/soft_nms_nvidia.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 +#include + +#include "centerpose_detection.hpp" + +namespace nvidia { +namespace isaac { +namespace centerpose { + +enum class NMSMethod : int { ORIGINAL = 0, LINEAR = 1, GAUSSIAN = 2 }; + +std::set SoftNMSNvidia( + const float threshold, const float sigma, const float Nt, const NMSMethod method, + CenterPoseDetectionList* src_detections); + +} // namespace centerpose +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_centerpose/gxf/centerpose/components/video_buffer_utils.hpp b/isaac_ros_centerpose/gxf/centerpose/components/video_buffer_utils.hpp new file mode 100644 index 0000000..5cef189 --- /dev/null +++ b/isaac_ros_centerpose/gxf/centerpose/components/video_buffer_utils.hpp @@ -0,0 +1,85 @@ +// 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 +#pragma once + +#include + +#include "gxf/multimedia/video.hpp" +#include "gxf/std/allocator.hpp" + +namespace nvidia { +namespace isaac { + +template +inline constexpr uint8_t GetChannelSize(); + +template <> +inline constexpr uint8_t GetChannelSize() { + return 3; +} + +template <> +inline constexpr uint8_t GetChannelSize() { + return 3; +} + +template <> +inline constexpr uint8_t GetChannelSize() { + return 1; +} + +template +inline const char* GetColorName(); + +template <> +inline const char* GetColorName() { + return "RGB"; +} + +template <> +inline const char* GetColorName() { + return "BGR"; +} + +template <> +inline const char* GetColorName() { + return "gray"; +} + +template +gxf::Expected AllocateVideoBuffer( + gxf::Handle video_buffer, size_t width, size_t height, + gxf::MemoryStorageType memory_storage_type, gxf::Handle allocator) { + if (width % 2 != 0 || height % 2 != 0) { + GXF_LOG_ERROR( + "Image width and height must be even, but received width: %zu, height: %zu", width, height); + return gxf::Unexpected{GXF_FAILURE}; + } + + std::array planes{ + gxf::ColorPlane(GetColorName(), GetChannelSize(), GetChannelSize() * width)}; + gxf::VideoFormatSize video_format_size; + const uint64_t size = video_format_size.size(width, height, planes); + const std::vector planes_filled{planes.begin(), planes.end()}; + const gxf::VideoBufferInfo buffer_info{ + static_cast(width), static_cast(height), T, planes_filled, + gxf::SurfaceLayout::GXF_SURFACE_LAYOUT_PITCH_LINEAR}; + return video_buffer->resizeCustom(buffer_info, size, memory_storage_type, allocator); +} + +} // namespace isaac +} // namespace nvidia diff --git a/isaac_ros_centerpose/include/isaac_ros_centerpose/centerpose_decoder_node.hpp b/isaac_ros_centerpose/include/isaac_ros_centerpose/centerpose_decoder_node.hpp new file mode 100644 index 0000000..479ff70 --- /dev/null +++ b/isaac_ros_centerpose/include/isaac_ros_centerpose/centerpose_decoder_node.hpp @@ -0,0 +1,65 @@ +// 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 + +#ifndef ISAAC_ROS_CENTERPOSE__CENTERPOSE_DECODER_NODE_HPP_ +#define ISAAC_ROS_CENTERPOSE__CENTERPOSE_DECODER_NODE_HPP_ + +#include +#include + +#include "isaac_ros_nitros/nitros_node.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace nvidia +{ +namespace isaac_ros +{ +namespace centerpose +{ + +/** + * @class CenterPoseDecoderNode + * @brief This node performs pose estimation of a known category from a single RGB image + * + */ +class CenterPoseDecoderNode : public nitros::NitrosNode +{ +public: + explicit CenterPoseDecoderNode(rclcpp::NodeOptions options = rclcpp::NodeOptions()); + ~CenterPoseDecoderNode(); + + void postLoadGraphCallback() override; + +private: + // 2D keypoint decoding size. Width and then height. + std::vector output_field_size_; + + // Scaling factor for cuboid + double cuboid_scaling_factor_; + + // Score threshold + double score_threshold_; + + // Object / instance name that is detected + std::string object_name_; +}; + +} // namespace centerpose +} // namespace isaac_ros +} // namespace nvidia + +#endif // ISAAC_ROS_CENTERPOSE__CENTERPOSE_DECODER_NODE_HPP_ diff --git a/isaac_ros_centerpose/include/isaac_ros_centerpose/centerpose_visualizer_node.hpp b/isaac_ros_centerpose/include/isaac_ros_centerpose/centerpose_visualizer_node.hpp new file mode 100644 index 0000000..350f321 --- /dev/null +++ b/isaac_ros_centerpose/include/isaac_ros_centerpose/centerpose_visualizer_node.hpp @@ -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 + +#ifndef ISAAC_ROS_CENTERPOSE__CENTERPOSE_VISUALIZER_NODE_HPP_ +#define ISAAC_ROS_CENTERPOSE__CENTERPOSE_VISUALIZER_NODE_HPP_ + +#include +#include + +#include "isaac_ros_nitros/nitros_node.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace nvidia +{ +namespace isaac_ros +{ +namespace centerpose +{ + +class CenterPoseVisualizerNode : public nitros::NitrosNode +{ +public: + explicit CenterPoseVisualizerNode(rclcpp::NodeOptions options = rclcpp::NodeOptions()); + ~CenterPoseVisualizerNode(); + + void postLoadGraphCallback() override; + +private: + // Whether to draw the axes onto the pose or not + bool show_axes_; + + // The bounding box color + int64_t bounding_box_color_; +}; + +} // namespace centerpose +} // namespace isaac_ros +} // namespace nvidia + +#endif // ISAAC_ROS_CENTERPOSE__CENTERPOSE_VISUALIZER_NODE_HPP_ diff --git a/isaac_ros_centerpose/isaac_ros_centerpose/CenterPoseDecoder.py b/isaac_ros_centerpose/isaac_ros_centerpose/CenterPoseDecoder.py deleted file mode 100644 index f6600e0..0000000 --- a/isaac_ros_centerpose/isaac_ros_centerpose/CenterPoseDecoder.py +++ /dev/null @@ -1,350 +0,0 @@ -# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2021-2022 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 - -from isaac_ros_centerpose.CenterPoseDecoderUtils import Cuboid3d, CuboidPNPSolver, \ - merge_outputs, nms, object_pose_post_process, tensor_to_numpy_array, \ - topk, topk_channel, transpose_and_gather_feat -from isaac_ros_tensor_list_interfaces.msg import TensorList -import numpy as np -import rclpy -from rclpy.duration import Duration -from rclpy.node import Node -from scipy import special -import torch -from visualization_msgs.msg import Marker, MarkerArray - - -# Order and shape of input tensors -TENSOR_IDX = {'hm': 0, - 'wh': 1, - 'hps': 2, - 'reg': 3, - 'hm_hp': 4, - 'hp_offset': 5, - 'scale': 6} - -TENSOR_SHAPE = {'hm': (1, 1, 128, 128), - 'wh': (1, 2, 128, 128), - 'hps': (1, 16, 128, 128), - 'hm_hp': (1, 8, 128, 128), - 'reg': (1, 2, 128, 128), - 'hp_offset': (1, 2, 128, 128), - 'scale': (1, 3, 128, 128)} - -# Constants -K = 100 -CONF_THRESH = 0.3 - - -def decode_impl(hm, wh, kps, hm_hp, reg, hp_offset, obj_scale, K): - hm = special.expit(hm) - hm_hp = special.expit(hm_hp) - - batch, _, _, _ = hm.shape - num_joints = kps.shape[1] // 2 - - hm = nms(hm) - scores, inds, clses, ys, xs = topk(hm, K=K) - - kps = transpose_and_gather_feat(kps, inds) - # joint offset from the centroid loc - kps = np.reshape(kps, (batch, K, num_joints*2)) - kps[:, :, ::2] += np.broadcast_to(np.reshape(xs, - (batch, K, 1)), (batch, K, num_joints)) - kps[:, :, 1::2] += np.broadcast_to(np.reshape(ys, - (batch, K, 1)), (batch, K, num_joints)) - - reg = transpose_and_gather_feat(reg, inds) - reg = np.reshape(reg, (batch, K, 2)) - xs = np.reshape(xs, (batch, K, 1)) + reg[:, :, 0:1] - ys = np.reshape(ys, (batch, K, 1)) + reg[:, :, 1:2] - - wh = transpose_and_gather_feat(wh, inds) - wh = np.reshape(wh, (batch, K, 2)) - - clses = np.reshape(clses, (batch, K, 1)).astype(float) - scores = np.reshape(scores, (batch, K, 1)) - - bboxes = np.concatenate([xs - wh[:, :, 0:1] / 2, - ys - wh[:, :, 1:2] / 2, - xs + wh[:, :, 0:1] / 2, - ys + wh[:, :, 1:2] / 2], axis=2) - - hm_hp = nms(hm_hp) - kps = np.transpose(np.reshape( - kps, (batch, K, num_joints, 2)), (0, 2, 1, 3)) # b x J x K x 2 - reg_kps = np.broadcast_to(np.expand_dims( - kps, 3), (batch, num_joints, K, K, 2)) # b x J x K x K x 2 - - hm_score, hm_inds, hm_ys, hm_xs = topk_channel(hm_hp, K=K) # b x J x K - if hp_offset is not None: - hp_offset = transpose_and_gather_feat( - hp_offset, np.reshape(hm_inds, (batch, -1))) - hp_offset = np.reshape(hp_offset, (batch, num_joints, K, 2)) - hm_xs = hm_xs + hp_offset[:, :, :, 0] - hm_ys = hm_ys + hp_offset[:, :, :, 1] - else: - hm_xs = hm_xs + 0.5 - hm_ys = hm_ys + 0.5 - - thresh = 0.1 - mask = (hm_score > thresh).astype(float) - hm_score = (1 - mask) * -1 + mask * hm_score # -1 or hm_score - hm_ys = (1 - mask) * (-10000) + mask * hm_ys # -10000 or hm_ys - hm_xs = (1 - mask) * (-10000) + mask * hm_xs - - hm_kps = np.broadcast_to(np.expand_dims(np.stack([hm_xs, hm_ys], axis=-1), 2), - (batch, num_joints, K, K, 2)) - - dist = (((reg_kps - hm_kps) ** 2).sum(axis=4) ** 0.5) # b x J x K x K - min_dist = dist.min(axis=3) # b x J x K - min_ind = dist.argmin(axis=3) # b x J x K - hm_score = np.expand_dims(torch.gather(torch.from_numpy( - hm_score), dim=2, index=torch.from_numpy(min_ind)).numpy(), -1) - min_dist = np.expand_dims(min_dist, -1) - min_ind = np.broadcast_to(np.reshape(min_ind, (num_joints, K, 1, 1)), - (batch, num_joints, K, 1, 2)) - - # make hm_kps and min_ind writable - hm_kps.setflags(write=1) - min_ind.setflags(write=1) - - hm_kps = torch.gather(torch.from_numpy(hm_kps), dim=3, - index=torch.from_numpy(min_ind)).numpy() - hm_kps = np.reshape(hm_kps, (batch, num_joints, K, 2)) - - left = np.broadcast_to(np.reshape( - bboxes[:, :, 0], (batch, 1, K, 1)), (batch, num_joints, K, 1)) - top = np.broadcast_to(np.reshape( - bboxes[:, :, 1], (batch, 1, K, 1)), (batch, num_joints, K, 1)) - right = np.broadcast_to(np.reshape( - bboxes[:, :, 2], (batch, 1, K, 1)), (batch, num_joints, K, 1)) - bottom = np.broadcast_to(np.reshape( - bboxes[:, :, 3], (batch, 1, K, 1)), (batch, num_joints, K, 1)) - mask = (hm_kps[:, :, 0:1] < left) + (hm_kps[:, :, 0:1] > right) + \ - (hm_kps[:, :, 1:2] < top) + (hm_kps[:, :, 1:2] > bottom) + \ - (hm_score < thresh) + (min_dist > - (np.maximum(bottom - top, right - left) * 0.3)) - mask = np.broadcast_to((mask > 0).astype(float), (batch, num_joints, K, 2)) - kps = (1 - mask) * hm_kps + mask * kps - kps = np.reshape(np.transpose(kps, (0, 2, 1, 3)), - (batch, K, num_joints * 2)) - obj_scale = transpose_and_gather_feat(obj_scale, inds) - obj_scale = np.reshape(obj_scale, (batch, K, 3)) - - detections = np.concatenate( - [bboxes, scores, kps, clses, obj_scale], axis=2) - return detections - - -def post_process_impl(res, params_config): - dets = res.reshape(1, -1, res.shape[2]) - dets = object_pose_post_process( - dets.copy(), - [np.divide(params_config['original_image_size'], 2)], [ - params_config['original_image_size'][0]], - params_config['output_field_size'][0], params_config['output_field_size'][1]) - dets[0][1] = np.array(dets[0][1], dtype=np.float32).reshape(-1, 25) - results = merge_outputs(dets) - - detected_objects = [] - - for bbox in results[1]: - if bbox[4] > CONF_THRESH: - pnp_solver = CuboidPNPSolver( - cuboid3d=Cuboid3d( - params_config['height'] * np.array(bbox[22:25])) - ) - - pnp_solver.set_camera_intrinsic_matrix( - params_config['camera_matrix']) - points = np.array(bbox[5:21]).reshape(-1, 2) - points = [(x[0], x[1]) for x in points] - location, quaternion, projected_points = pnp_solver.solve_pnp( - points) - detected_objects.append({ - 'location': location, - 'quaternion': quaternion, - 'confidence': bbox[4], - 'cuboid_dimensions': params_config['height'] * np.array(bbox[22:25]), - }) - - return detected_objects - - -def decode(t_hm, t_wh, t_kps, t_hm_hp, t_reg, t_hp_offset, t_obj_scale, K, params_config): - # Convert tensors to numpy arrays - hm = tensor_to_numpy_array(t_hm) - wh = tensor_to_numpy_array(t_wh) - kps = tensor_to_numpy_array(t_kps) - hm_hp = tensor_to_numpy_array(t_hm_hp) - reg = tensor_to_numpy_array(t_reg) - hp_offset = tensor_to_numpy_array(t_hp_offset) - obj_scale = tensor_to_numpy_array(t_obj_scale) - - # Decode tensors into a set of detections - res = decode_impl(hm, wh, kps, hm_hp, reg, hp_offset, obj_scale, K) - # Post-process the detections into 3D cuboid bounding boxes and poses - detected_objects = post_process_impl(res, params_config) - - objects = MarkerArray() - for idx, obj in enumerate(detected_objects): - mm = Marker() - mm.id = idx - mm.type = Marker.CUBE - mm.action = 0 # add a detection - mm.pose.position.x = obj['location'][0] - mm.pose.position.y = obj['location'][1] - mm.pose.position.z = obj['location'][2] - mm.pose.orientation.x = obj['quaternion'][0] - mm.pose.orientation.y = obj['quaternion'][1] - mm.pose.orientation.z = obj['quaternion'][2] - mm.pose.orientation.w = obj['quaternion'][3] - mm.scale.x = obj['cuboid_dimensions'][0] - mm.scale.y = obj['cuboid_dimensions'][1] - mm.scale.z = obj['cuboid_dimensions'][2] - mm.lifetime = Duration(nanoseconds=33333333.3333).to_msg() # 30 fps - mm.color.r = float(params_config['marker_color'][0]) - mm.color.g = float(params_config['marker_color'][1]) - mm.color.b = float(params_config['marker_color'][2]) - mm.color.a = float(params_config['marker_color'][3]) - objects.markers.append(mm) - - return objects - - -class IsaacROSCenterposeDecoderNode(Node): - - def __init__(self, name='centerpose_decoder_node'): - super().__init__(name) - self.declare_parameters( - namespace='', - parameters=[ - ('camera_matrix', rclpy.Parameter.Type.DOUBLE_ARRAY), - ('original_image_size', rclpy.Parameter.Type.INTEGER_ARRAY), - ('output_field_size', rclpy.Parameter.Type.INTEGER_ARRAY), - ('height', rclpy.Parameter.Type.DOUBLE), - ('frame_id', 'centerpose'), - ('marker_color', rclpy.Parameter.Type.DOUBLE_ARRAY) - ] - ) - # Sanity check parameters - self.params_config = {} - param_names = ['camera_matrix', 'original_image_size', - 'output_field_size', 'height', 'marker_color', - 'frame_id'] - for param_name in param_names: - try: - self.params_config[param_name] = self.get_parameter( - param_name).value - except rclpy.exceptions.ParameterUninitializedException: - self.params_config[param_name] = None - - if (self.params_config['camera_matrix'] is None) or \ - (len(self.params_config['camera_matrix']) != 9): - self.get_logger().warning('No camera matrix specified. ' - 'Pose estimates will be inaccurate') - # These are the intrinsic camera parameters used to train the - # NVIDIA-provided pretrained models. - self.params_config['camera_matrix'] = np.matrix( - [[616.078125, 0, 325.8349304199219], - [0, 616.1030883789062, 244.4612274169922], - [0, 0, 1]]) - else: - self.params_config['camera_matrix'] = np.matrix( - self.params_config['camera_matrix']).reshape(3, 3) - - if (self.params_config['original_image_size'] is None) or \ - (len(self.params_config['original_image_size']) != 2): - self.get_logger().warning('No original image size specified. ' - 'Pose estimates will be inaccurate') - self.params_config['original_image_size'] = np.array([640, 480]) - - if (self.params_config['output_field_size'] is None) or \ - (len(self.params_config['output_field_size']) != 2): - self.get_logger().warning('No output field size specified. Assuming 128x128') - self.params_config['output_field_size'] = np.array([128, 128]) - - if self.params_config['height'] is None: - self.get_logger().warning('No height specified. Assuming 1.0') - self.params_config['height'] = 1.0 - - if self.params_config['marker_color'] is None or \ - (len(self.params_config['marker_color']) != 4): - self.get_logger().warning( - 'No marker color of correct size specified. Assuming RGBA of (1.0, 0.0, 0.0, 1.0)') - self.params_config['marker_color'] = [1.0, 0.0, 0.0, 1.0] - - # Create the subscriber. This subscriber will subscribe to a TensorList message - self.subscription_ = self.create_subscription(TensorList, 'tensor_sub', - self.listener_callback, 10) - - # Create the publisher. This publisher will publish a MarkerArray message - # to a topic. The queue size is 10 messages. - self.publisher_ = self.create_publisher( - MarkerArray, 'object_poses', 10) - - def listener_callback(self, msg): - tensors = msg.tensors - if len(tensors) != len(TENSOR_IDX): - self.get_logger().error('Received tensor not of correct length. ' - 'Expected %s, got %s' % (len(TENSOR_IDX), len(tensors))) - return - for key in TENSOR_IDX.keys(): - if tuple(tensors[TENSOR_IDX[key]].shape.dims) != TENSOR_SHAPE[key]: - self.get_logger().error( - 'Tensor %s not of correct shape. Expected %s, got %s' % - (key, TENSOR_SHAPE[key], tensors[TENSOR_IDX[key]].shape.dims)) - return - - poses = decode(tensors[TENSOR_IDX['hm']], - tensors[TENSOR_IDX['wh']], - tensors[TENSOR_IDX['hps']], - tensors[TENSOR_IDX['hm_hp']], - tensors[TENSOR_IDX['reg']], - tensors[TENSOR_IDX['hp_offset']], - tensors[TENSOR_IDX['scale']], - K, self.params_config) - - if poses is None: - self.get_logger().error('Error decoding input tensors') - return - - for pose in poses.markers: - pose.header = msg.header - pose.header.frame_id = self.params_config['frame_id'] - - # Publish the message to the topic - self.publisher_.publish(poses) - - -def main(args=None): - try: - rclpy.init(args=args) - node = IsaacROSCenterposeDecoderNode('centerpose_decoder_node') - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - # only shut down if context is active - if rclpy.ok(): - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/isaac_ros_centerpose/isaac_ros_centerpose/CenterPoseDecoderUtils.py b/isaac_ros_centerpose/isaac_ros_centerpose/CenterPoseDecoderUtils.py deleted file mode 100644 index 085a764..0000000 --- a/isaac_ros_centerpose/isaac_ros_centerpose/CenterPoseDecoderUtils.py +++ /dev/null @@ -1,522 +0,0 @@ -# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2021-2022 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 -from enum import IntEnum - -import cv2 -import numpy as np -import torch -from torch import nn - - -def tensor_to_numpy_array(tensor): - shape = tuple(tensor.shape.dims) - x = None - if tensor.data_type == 9: # float32 - x = np.frombuffer(bytearray(tensor.data), dtype='float32') - elif tensor.data_type == 10: # float64 - x = np.frombuffer(bytearray(tensor.data), dtype='float64') - else: - print('Received tensor of incorrect type:', tensor.data_type) - return None - x = np.reshape(x, shape) - return x - - -def quaternion_from_axis_rotation(axis, theta): - # Normalize axis vector if needed - if not np.isclose(np.linalg.norm(axis), 1.0): - axis = axis / np.linalg.norm(axis) - - half_theta = theta * 0.5 - sht = np.sin(half_theta) - - # Quaternion order is (x, y, z, w) - qq = np.array([sht*axis[0], sht*axis[1], sht*axis[2], np.cos(half_theta)]) - return qq.flatten() - - -def quaternion_cross(q1, q2): - q1x, q1y, q1z, q1w = q1 - q2x, q2y, q2z, q2w = q2 - - return np.array([q1x * q2w + q1y * q2z - q1z * q2y + q1w * q2x, - -q1x * q2z + q1y * q2w + q1z * q2x + q1w * q2y, - q1x * q2y - q1y * q2x + q1z * q2w + q1w * q2z, - -q1x * q2x - q1y * q2y - q1z * q2z + q1w * q2w], - dtype=q1.dtype) - - -def gather_feat(feat, ind, mask=None): - dim = feat.shape[2] - ind = np.broadcast_to(np.expand_dims( - ind, 2), (ind.shape[0], ind.shape[1], dim)) - # make ind writable - ind.setflags(write=1) - feat = torch.gather(torch.from_numpy(feat), 1, - torch.from_numpy(ind)).numpy() - - if mask is not None: - mask = np.broadcast_to(np.expand_dims(mask, 2), (feat.shape)) - feat = feat[mask] - feat = feat.view(-1, dim) - - return feat - - -def np_topk(a, k, axis=-1, is_sorted=True): - if axis is None: - axis_size = a.size - else: - axis_size = a.shape[axis] - assert 1 <= k <= axis_size - - a = np.asanyarray(a) - index_array = np.argpartition(a, axis_size-k, axis=axis) - topk_indices = np.take(index_array, -np.arange(k)-1, axis=axis) - topk_values = np.take_along_axis(a, topk_indices, axis=axis) - if is_sorted: - sorted_indices_in_topk = np.argsort(topk_values, axis=axis) - sorted_indices_in_topk = np.flip(sorted_indices_in_topk, axis=axis) - sorted_topk_values = np.take_along_axis( - topk_values, sorted_indices_in_topk, axis=axis) - sorted_topk_indices = np.take_along_axis( - topk_indices, sorted_indices_in_topk, axis=axis) - return sorted_topk_values, sorted_topk_indices - return topk_values, topk_indices - - -def topk(scores, K=40): - batch, cat, height, width = scores.shape - topk_scores, topk_inds = np_topk(np.reshape(scores, (batch, cat, -1)), K) - - topk_inds = topk_inds % (height * width) - topk_ys = (topk_inds // width).astype(float) - topk_xs = (topk_inds % width).astype(float) - - topk_score, topk_ind = np_topk(np.reshape(topk_scores, (batch, -1)), K) - topk_clses = (topk_ind // K).astype(int) - topk_inds = np.reshape(gather_feat(np.reshape( - topk_inds, (batch, -1, 1)), topk_ind), (batch, K)) - topk_ys = np.reshape(gather_feat(np.reshape( - topk_ys, (batch, -1, 1)), topk_ind), (batch, K)) - topk_xs = np.reshape(gather_feat(np.reshape( - topk_xs, (batch, -1, 1)), topk_ind), (batch, K)) - - return topk_score, topk_inds, topk_clses, topk_ys, topk_xs - - -def topk_channel(scores, K=40): - batch, cat, height, width = scores.shape - - topk_scores, topk_inds = np_topk(np.reshape(scores, (batch, cat, -1)), K) - - topk_inds = topk_inds % (height * width) - topk_ys = (topk_inds // width).astype(float) - topk_xs = (topk_inds % width).astype(float) - - return topk_scores, topk_inds, topk_ys, topk_xs - - -def nms(heat, kernel=3): - m = nn.MaxPool2d(kernel, stride=1, padding=1) - hmax = m(torch.from_numpy(heat)) - keep = 1.0 * (hmax.numpy() == heat) - return heat * keep - - -def transpose_and_gather_feat(feat, ind): - feat = np.transpose(feat, (0, 2, 3, 1)) - feat = np.reshape(feat, (feat.shape[0], -1, feat.shape[3])) - feat = gather_feat(feat, ind) - return feat - - -def get_3rd_point(a, b): - direct = a - b - return b + np.array([-direct[1], direct[0]], dtype=np.float32) - - -def get_dir(src_point, rot_rad): - sn, cs = np.sin(rot_rad), np.cos(rot_rad) - - src_result = [0, 0] - src_result[0] = src_point[0] * cs - src_point[1] * sn - src_result[1] = src_point[0] * sn + src_point[1] * cs - - return src_result - - -def get_affine_transform(center, - scale, - rot, - output_size, - shift=np.array([0, 0], dtype=np.float32), - inv=0): - if not isinstance(scale, np.ndarray) and not isinstance(scale, list): - scale = np.array([scale, scale], dtype=np.float32) - - scale_tmp = scale - src_w = scale_tmp[0] - dst_w = output_size[0] - dst_h = output_size[1] - - rot_rad = np.pi * rot / 180 - src_dir = get_dir([0, src_w * -0.5], rot_rad) - dst_dir = np.array([0, dst_w * -0.5], np.float32) - - src = np.zeros((3, 2), dtype=np.float32) - dst = np.zeros((3, 2), dtype=np.float32) - src[0, :] = center + scale_tmp * shift - src[1, :] = center + src_dir + scale_tmp * shift - dst[0, :] = [dst_w * 0.5, dst_h * 0.5] - dst[1, :] = np.array([dst_w * 0.5, dst_h * 0.5], np.float32) + dst_dir - - src[2:, :] = get_3rd_point(src[0, :], src[1, :]) - dst[2:, :] = get_3rd_point(dst[0, :], dst[1, :]) - - if inv: - trans = cv2.getAffineTransform(np.float32(dst), np.float32(src)) - else: - trans = cv2.getAffineTransform(np.float32(src), np.float32(dst)) - - return trans - - -def affine_transform(pt, t): - new_pt = np.array([pt[0], pt[1], 1.], dtype=np.float32).T - new_pt = np.dot(t, new_pt) - return new_pt[:2] - - -def transform_preds(coords, center, scale, output_size): - target_coords = np.zeros(coords.shape) - trans = get_affine_transform(center, scale, 0, output_size, inv=1) - for p in range(coords.shape[0]): - target_coords[p, 0:2] = affine_transform(coords[p, 0:2], trans) - return target_coords - - -def object_pose_post_process(dets, c, s, h, w): - # Scale bbox & pts - ret = [] - for i in range(dets.shape[0]): - bbox = transform_preds( - dets[i, :, :4].reshape(-1, 2), c[i], s[i], (w, h)) - pts = transform_preds( - dets[i, :, 5:21].reshape(-1, 2), c[i], s[i], (w, h)) - - top_preds = np.concatenate( - [bbox.reshape(-1, 4), dets[i, :, 4:5], - pts.reshape(-1, 16), - dets[i, :, 21:25] - ], axis=1).astype(np.float32).tolist() - ret.append({np.ones(1, dtype=np.int32)[0]: top_preds}) - return ret - - -def merge_outputs(detections): - results = {} - results[1] = np.concatenate( - [detection[1] for detection in detections], axis=0).astype(np.float32) - results[1] = results[1].tolist() - return results - - -class CuboidVertexType(IntEnum): - """Define an object's local coordinate system.""" - - FrontTopRight = 0 - FrontTopLeft = 1 - FrontBottomLeft = 2 - FrontBottomRight = 3 - RearTopRight = 4 - RearTopLeft = 5 - RearBottomLeft = 6 - RearBottomRight = 7 - Center = 8 - TotalCornerVertexCount = 8 # Corner vertices don't include the center point - TotalVertexCount = 9 - - -# List of the vertex indexes in each edge of the cuboid -CuboidLineIndexes = [ - # Front face - [CuboidVertexType.FrontTopLeft, CuboidVertexType.FrontTopRight], - [CuboidVertexType.FrontTopRight, CuboidVertexType.FrontBottomRight], - [CuboidVertexType.FrontBottomRight, CuboidVertexType.FrontBottomLeft], - [CuboidVertexType.FrontBottomLeft, CuboidVertexType.FrontTopLeft], - # Back face - [CuboidVertexType.RearTopLeft, CuboidVertexType.RearTopRight], - [CuboidVertexType.RearTopRight, CuboidVertexType.RearBottomRight], - [CuboidVertexType.RearBottomRight, CuboidVertexType.RearBottomLeft], - [CuboidVertexType.RearBottomLeft, CuboidVertexType.RearTopLeft], - # Left face - [CuboidVertexType.FrontBottomLeft, CuboidVertexType.RearBottomLeft], - [CuboidVertexType.FrontTopLeft, CuboidVertexType.RearTopLeft], - # Right face - [CuboidVertexType.FrontBottomRight, CuboidVertexType.RearBottomRight], - [CuboidVertexType.FrontTopRight, CuboidVertexType.RearTopRight], -] - - -class Cuboid3d(): - """This class defines a 3D cuboid.""" - - # Create a box with a certain size - def __init__(self, size3d=[1.0, 1.0, 1.0]): - self.center_location = [0, 0, 0] - self.size3d = size3d - self._vertices = [0, 0, 0] * CuboidVertexType.TotalCornerVertexCount - self.generate_vertices() - - def get_vertex(self, vertex_type): - """Return the location of a vertex. - - Args: - vertex_type: enum of type CuboidVertexType - Returns: - Numpy array(3) - Location of the vertex type in the cuboid - """ - return self._vertices[vertex_type] - - def get_vertices(self): - return self._vertices - - def generate_vertices(self): - width, height, depth = self.size3d - - # By default just use the normal OpenCV coordinate system - cx, cy, cz = self.center_location - # X axis point to the right - right = cx + width / 2.0 - left = cx - width / 2.0 - # Y axis point upward - top = cy + height / 2.0 - bottom = cy - height / 2.0 - # Z axis point forward - front = cz + depth / 2.0 - rear = cz - depth / 2.0 - - # List of 8 vertices of the box - self._vertices = [ - [left, bottom, rear], # Rear Bottom Left - [left, bottom, front], # Front Bottom Left - [left, top, rear], # Rear Top Left - [left, top, front], # Front Top Left - - [right, bottom, rear], # Rear Bottom Right - [right, bottom, front], # Front Bottom Right - [right, top, rear], # Rear Top Right - [right, top, front], # Front Top Right - ] - - -class CuboidPNPSolver(object): - """ - This class is used to find the 6-DoF pose of a cuboid given its projected vertices. - - Runs perspective-n-point (PNP) algorithm. - """ - - # Class variables - cv2version = cv2.__version__.split('.') - cv2majorversion = int(cv2version[0]) - - def __init__(self, - scaling_factor=1, - camera_intrinsic_matrix=None, - cuboid3d=None, - dist_coeffs=np.zeros((4, 1)), - min_required_points=4 - ): - self.min_required_points = max(4, min_required_points) - self.scaling_factor = scaling_factor - - if camera_intrinsic_matrix is not None: - self._camera_intrinsic_matrix = camera_intrinsic_matrix - else: - self._camera_intrinsic_matrix = np.array([ - [0, 0, 0], - [0, 0, 0], - [0, 0, 0] - ]) - self._cuboid3d = cuboid3d - self._dist_coeffs = dist_coeffs - - def set_camera_intrinsic_matrix(self, new_intrinsic_matrix): - """Set the camera intrinsic matrix.""" - self._camera_intrinsic_matrix = new_intrinsic_matrix - - def set_dist_coeffs(self, dist_coeffs): - """Set the camera intrinsic matrix.""" - self._dist_coeffs = dist_coeffs - - def __check_pnp_result(self, - points, - projected_points, - fail_if_projected_diff_exceeds, - fail_if_projected_value_exceeds): - """ - Check whether the output of PNP seems reasonable. - - Inputs: - - cuboid2d_points: list of XY tuples - - projected points: np.ndarray of np.ndarrays - """ - p1 = points - p2 = projected_points.tolist() - assert len(p1) == len(p2) - - # Compute max Euclidean 2D distance b/w points and projected points - max_euclidean_dist = 0 - for i in range(len(p1)): - if p1[i] is not None: - dist = np.linalg.norm(np.array(p1[i]) - np.array(p2[i])) - if dist > max_euclidean_dist: - max_euclidean_dist = dist - - # Compute max projected absolute value - max_abs_value = 0 - for i in range(len(p2)): - assert len(p2[i]) == 2 - for val in p2[i]: - if val > max_abs_value: - max_abs_value = val - - # Return success (true) or failure (false) - return max_euclidean_dist <= fail_if_projected_diff_exceeds \ - and max_abs_value <= fail_if_projected_value_exceeds - - def solve_pnp(self, - cuboid2d_points, - pnp_algorithm=None, - fail_if_projected_diff_exceeds=250, - fail_if_projected_value_exceeds=1e5 - ): - """ - Detect the rotation and traslation of a cuboid object. - - Inputs: - - cuboid2d_points: list of XY tuples - ... - - Outputs: - - location in 3D - - pose in 3D (as quaternion) - - projected points: np.ndarray of np.ndarrays - - """ - # Fallback to default PNP algorithm base on OpenCV version - if pnp_algorithm is None: - if CuboidPNPSolver.cv2majorversion == 2: - pnp_algorithm = cv2.CV_ITERATIVE - elif CuboidPNPSolver.cv2majorversion == 3: - pnp_algorithm = cv2.SOLVEPNP_ITERATIVE - # Alternative algorithms: - # pnp_algorithm = SOLVE_PNP_P3P - # pnp_algorithm = SOLVE_PNP_EPNP - else: - pnp_algorithm = cv2.SOLVEPNP_EPNP - - location = None - quaternion = None - projected_points = cuboid2d_points - cuboid3d_points = np.array(self._cuboid3d.get_vertices()) - obj_2d_points = [] - obj_3d_points = [] - - for i in range(CuboidVertexType.TotalCornerVertexCount): - check_point_2d = cuboid2d_points[i] - # Ignore invalid points - if (check_point_2d is None): - continue - obj_2d_points.append(check_point_2d) - obj_3d_points.append(cuboid3d_points[i]) - - obj_2d_points = np.array(obj_2d_points, dtype=float) - obj_3d_points = np.array(obj_3d_points, dtype=float) - - valid_point_count = len(obj_2d_points) - - # Can only do PNP if we have more than 3 valid points - is_points_valid = valid_point_count >= self.min_required_points - - if is_points_valid: - ret, rvec, tvec = cv2.solvePnP( - obj_3d_points, - obj_2d_points, - self._camera_intrinsic_matrix, - self._dist_coeffs, - flags=pnp_algorithm - ) - - if ret: - # OpenCV result - location = [x[0] for x in tvec] - quaternion = self.convert_rvec_to_quaternion(rvec) - - # Use OpenCV to project 3D points - projected_points, _ = cv2.projectPoints(cuboid3d_points, rvec, tvec, - self._camera_intrinsic_matrix, - self._dist_coeffs) - projected_points = np.squeeze(projected_points) - - success = self.__check_pnp_result( - cuboid2d_points, - projected_points, - fail_if_projected_diff_exceeds, - fail_if_projected_value_exceeds) - - # If the location.Z is negative or object is behind the camera, - # flip both location and rotation - x, y, z = location - if z < 0 or not success: - # Get the opposite location - location = [-x, -y, -z] - - # Change the rotation by 180 degree - rotate_angle = np.pi - rotate_quaternion = quaternion_from_axis_rotation( - location, rotate_angle) - quaternion = quaternion_cross( - rotate_quaternion, quaternion) - - # Quaternion ordering: (x, y, z, w) - return location, \ - [quaternion[0], quaternion[1], quaternion[2], quaternion[3]], \ - projected_points - - def convert_rvec_to_quaternion(self, rvec): - """Convert rvec (which is log quaternion) to quaternion.""" - theta = np.sqrt(rvec[0] * rvec[0] + rvec[1] * - rvec[1] + rvec[2] * rvec[2]) # in radians - raxis = [rvec[0] / theta, rvec[1] / theta, rvec[2] / theta] - - # Quaternion order is (x, y, z, w) - return quaternion_from_axis_rotation(raxis, theta) - - def project_points(self, rvec, tvec): - """Project points from model onto image using rotation, translation.""" - output_points, tmp = cv2.projectPoints( - self.__object_vertex_coordinates, - rvec, - tvec, - self.__camera_intrinsic_matrix, - self.__dist_coeffs) - - output_points = np.squeeze(output_points) - return output_points diff --git a/isaac_ros_centerpose/launch/isaac_ros_centerpose_tensor_rt.launch.py b/isaac_ros_centerpose/launch/isaac_ros_centerpose_tensor_rt.launch.py new file mode 100644 index 0000000..90cd607 --- /dev/null +++ b/isaac_ros_centerpose/launch/isaac_ros_centerpose_tensor_rt.launch.py @@ -0,0 +1,232 @@ +# 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 + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + """Generate launch description for Centerpose.""" + tensor_names = [ + 'bboxes', + 'scores', + 'kps', + 'clses', + 'obj_scale', + 'kps_displacement_mean', + 'kps_heatmap_mean', + ] + tensor_names_str = '[' + for tensor_name in tensor_names: + tensor_names_str += f'"{tensor_name}"' + if tensor_name != tensor_names[-1]: + tensor_names_str += ',' + tensor_names_str += ']' + + launch_args = [ + DeclareLaunchArgument( + 'input_image_width', + default_value='600', + description='The input image width'), + DeclareLaunchArgument( + 'input_image_height', + default_value='800', + description='The input image height'), + DeclareLaunchArgument( + 'network_image_width', + default_value='512', + description='The input image width that the network expects'), + DeclareLaunchArgument( + 'network_image_height', + default_value='512', + description='The input image height that the network expects'), + DeclareLaunchArgument( + 'encoder_image_mean', + default_value='[0.408, 0.447, 0.47]', + description='The mean for image normalization'), + DeclareLaunchArgument( + 'encoder_image_stddev', + default_value='[0.289, 0.274, 0.278]', + description='The standard deviation for image normalization'), + DeclareLaunchArgument( + 'model_file_path', + default_value='', + description='The absolute file path to the ONNX file'), + DeclareLaunchArgument( + 'engine_file_path', + default_value='', + description='The absolute file path to the TensorRT engine file'), + DeclareLaunchArgument( + 'input_tensor_names', + default_value='["input_tensor"]', + description='A list of tensor names to bound to the specified input binding names'), + DeclareLaunchArgument( + 'input_binding_names', + default_value='["input"]', + description='A list of input tensor binding names (specified by model)'), + DeclareLaunchArgument( + 'input_tensor_formats', + default_value='["nitros_tensor_list_nchw_rgb_f32"]', + description='The nitros format of the input tensors'), + DeclareLaunchArgument( + 'output_tensor_names', + default_value=tensor_names_str, + description='A list of tensor names to bound to the specified output binding names'), + DeclareLaunchArgument( + 'output_binding_names', + default_value=tensor_names_str, + description='A list of output tensor binding names (specified by model)'), + DeclareLaunchArgument( + 'output_tensor_formats', + default_value='["nitros_tensor_list_nhwc_rgb_f32"]', + description='The nitros format of the output tensors'), + DeclareLaunchArgument( + 'tensorrt_verbose', + default_value='False', + description='Whether TensorRT should verbosely log or not'), + DeclareLaunchArgument( + 'force_engine_update', + default_value='False', + description='Whether TensorRT should update the TensorRT engine file or not'), + DeclareLaunchArgument( + 'output_field_size', + default_value='[128, 128]', + description='Represents the size of the 2D keypoint decoding from the network output'), + DeclareLaunchArgument( + 'cuboid_scaling_factor', + default_value='1.0', + description='Scales the cuboid used for calculating the size of the objects detected'), + DeclareLaunchArgument( + 'score_threshold', + default_value='0.3', + description='The threshold for scores values to discard.'), + DeclareLaunchArgument( + 'object_name', + default_value='shoe', + description='The name of the category instance that is being detected', + ), + DeclareLaunchArgument( + 'show_axes', + default_value='True', + description='Whether to show axes or not for visualization', + ), + DeclareLaunchArgument( + 'bounding_box_color', + default_value='0x000000ff', + description='The color of the bounding box for visualization', + ), + ] + + # DNN Image Encoder parameters + input_image_width = LaunchConfiguration('input_image_width') + input_image_height = LaunchConfiguration('input_image_height') + network_image_width = LaunchConfiguration('network_image_width') + network_image_height = LaunchConfiguration('network_image_height') + encoder_image_mean = LaunchConfiguration('encoder_image_mean') + encoder_image_stddev = LaunchConfiguration('encoder_image_stddev') + + # Tensor RT parameters + model_file_path = LaunchConfiguration('model_file_path') + engine_file_path = LaunchConfiguration('engine_file_path') + input_tensor_names = LaunchConfiguration('input_tensor_names') + input_binding_names = LaunchConfiguration('input_binding_names') + input_tensor_formats = LaunchConfiguration('input_tensor_formats') + output_tensor_names = LaunchConfiguration('output_tensor_names') + output_binding_names = LaunchConfiguration('output_binding_names') + output_tensor_formats = LaunchConfiguration('output_tensor_formats') + tensorrt_verbose = LaunchConfiguration('tensorrt_verbose') + force_engine_update = LaunchConfiguration('force_engine_update') + + # Centerpose Decoder parameters + output_field_size = LaunchConfiguration('output_field_size') + cuboid_scaling_factor = LaunchConfiguration('cuboid_scaling_factor') + score_threshold = LaunchConfiguration('score_threshold') + object_name = LaunchConfiguration('object_name') + + # Centerpose visualization parameters + show_axes = LaunchConfiguration('show_axes') + bounding_box_color = LaunchConfiguration('bounding_box_color') + + centerpose_encoder_node = ComposableNode( + name='centerpose_encoder', + package='isaac_ros_dnn_image_encoder', + plugin='nvidia::isaac_ros::dnn_inference::DnnImageEncoderNode', + parameters=[{ + 'input_image_width': input_image_width, + 'input_image_height': input_image_height, + 'network_image_width': network_image_width, + 'network_image_height': network_image_height, + 'image_mean': encoder_image_mean, + 'image_stddev': encoder_image_stddev, + }], + remappings=[('encoded_tensor', 'tensor_pub')]) + + centerpose_inference_node = ComposableNode( + name='centerpose_inference', + package='isaac_ros_tensor_rt', + plugin='nvidia::isaac_ros::dnn_inference::TensorRTNode', + parameters=[{ + 'model_file_path': model_file_path, + 'engine_file_path': engine_file_path, + 'input_tensor_names': input_tensor_names, + 'input_binding_names': input_binding_names, + 'input_tensor_formats': input_tensor_formats, + 'output_tensor_names': output_tensor_names, + 'output_binding_names': output_binding_names, + 'output_tensor_formats': output_tensor_formats, + 'verbose': tensorrt_verbose, + 'force_engine_update': force_engine_update + }]) + + centerpose_decoder_node = ComposableNode( + name='centerpose_decoder_node', + package='isaac_ros_centerpose', + plugin='nvidia::isaac_ros::centerpose::CenterPoseDecoderNode', + parameters=[{ + 'output_field_size': output_field_size, + 'cuboid_scaling_factor': cuboid_scaling_factor, + 'score_threshold': score_threshold, + 'object_name': object_name, + }], + ) + + centerpose_visualizer_node = ComposableNode( + name='centerpose_visualizer_node', + package='isaac_ros_centerpose', + plugin='nvidia::isaac_ros::centerpose::CenterPoseVisualizerNode', + parameters=[{ + 'show_axes': show_axes, + 'bounding_box_color': bounding_box_color, + }], + ) + + rclcpp_container = ComposableNodeContainer( + name='centerpose_container', + namespace='', + package='rclcpp_components', + executable='component_container_mt', + composable_node_descriptions=[ + centerpose_encoder_node, centerpose_inference_node, + centerpose_decoder_node, centerpose_visualizer_node], + output='screen', + ) + + final_launch_container = launch_args + [rclcpp_container] + return LaunchDescription(final_launch_container) diff --git a/isaac_ros_centerpose/launch/isaac_ros_centerpose.launch.py b/isaac_ros_centerpose/launch/isaac_ros_centerpose_triton.launch.py similarity index 60% rename from isaac_ros_centerpose/launch/isaac_ros_centerpose.launch.py rename to isaac_ros_centerpose/launch/isaac_ros_centerpose_triton.launch.py index c0f2dd6..f4a299c 100644 --- a/isaac_ros_centerpose/launch/isaac_ros_centerpose.launch.py +++ b/isaac_ros_centerpose/launch/isaac_ros_centerpose_triton.launch.py @@ -1,5 +1,5 @@ # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2021-2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# 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. @@ -15,25 +15,40 @@ # # SPDX-License-Identifier: Apache-2.0 -import os - -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer, Node +from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode def generate_launch_description(): """Generate launch description for Centerpose.""" - config = os.path.join( - get_package_share_directory('isaac_ros_centerpose'), - 'config', - 'decoder_params.yaml' - ) + tensor_names = [ + 'bboxes', + 'scores', + 'kps', + 'clses', + 'obj_scale', + 'kps_displacement_mean', + 'kps_heatmap_mean', + ] + tensor_names_str = '[' + for tensor_name in tensor_names: + tensor_names_str += f'"{tensor_name}"' + if tensor_name != tensor_names[-1]: + tensor_names_str += ',' + tensor_names_str += ']' launch_args = [ + DeclareLaunchArgument( + 'input_image_width', + default_value='600', + description='The input image width'), + DeclareLaunchArgument( + 'input_image_height', + default_value='800', + description='The input image height'), DeclareLaunchArgument( 'network_image_width', default_value='512', @@ -76,19 +91,48 @@ def generate_launch_description(): description='The nitros format of the input tensors'), DeclareLaunchArgument( 'output_tensor_names', - default_value='["hm", "wh", "hps", "reg", "hm_hp", "hp_offset", "scale"]', + default_value=tensor_names_str, description='A list of tensor names to bound to the specified output binding names'), DeclareLaunchArgument( 'output_binding_names', - default_value='["hm", "wh", "hps", "reg", "hm_hp", "hp_offset", "scale"]', - description='A list of output tensor binding names (specified by model)'), + default_value=tensor_names_str, + description='A list of output tensor binding names (specified by model)'), DeclareLaunchArgument( 'output_tensor_formats', default_value='["nitros_tensor_list_nhwc_rgb_f32"]', description='The nitros format of the output tensors'), + DeclareLaunchArgument( + 'output_field_size', + default_value='[128, 128]', + description='Represents the size of the 2D keypoint decoding from the network output'), + DeclareLaunchArgument( + 'cuboid_scaling_factor', + default_value='1.0', + description='Scales the cuboid used for calculating the size of the objects detected'), + DeclareLaunchArgument( + 'score_threshold', + default_value='0.3', + description='The threshold for scores values to discard.'), + DeclareLaunchArgument( + 'object_name', + default_value='shoe', + description='The name of the category instance that is being detected', + ), + DeclareLaunchArgument( + 'show_axes', + default_value='True', + description='Whether to show axes or not for visualization', + ), + DeclareLaunchArgument( + 'bounding_box_color', + default_value='0x000000ff', + description='The color of the bounding box for visualization', + ), ] # DNN Image Encoder parameters + input_image_width = LaunchConfiguration('input_image_width') + input_image_height = LaunchConfiguration('input_image_height') network_image_width = LaunchConfiguration('network_image_width') network_image_height = LaunchConfiguration('network_image_height') encoder_image_mean = LaunchConfiguration('encoder_image_mean') @@ -105,11 +149,23 @@ def generate_launch_description(): output_binding_names = LaunchConfiguration('output_binding_names') output_tensor_formats = LaunchConfiguration('output_tensor_formats') + # Centerpose Decoder parameters + output_field_size = LaunchConfiguration('output_field_size') + cuboid_scaling_factor = LaunchConfiguration('cuboid_scaling_factor') + score_threshold = LaunchConfiguration('score_threshold') + object_name = LaunchConfiguration('object_name') + + # Centerpose visualization parameters + show_axes = LaunchConfiguration('show_axes') + bounding_box_color = LaunchConfiguration('bounding_box_color') + centerpose_encoder_node = ComposableNode( name='centerpose_encoder', - package='isaac_ros_dnn_encoders', + package='isaac_ros_dnn_image_encoder', plugin='nvidia::isaac_ros::dnn_inference::DnnImageEncoderNode', parameters=[{ + 'input_image_width': input_image_width, + 'input_image_height': input_image_height, 'network_image_width': network_image_width, 'network_image_height': network_image_height, 'image_mean': encoder_image_mean, @@ -133,24 +189,38 @@ def generate_launch_description(): 'output_tensor_formats': output_tensor_formats, }]) + centerpose_decoder_node = ComposableNode( + name='centerpose_decoder_node', + package='isaac_ros_centerpose', + plugin='nvidia::isaac_ros::centerpose::CenterPoseDecoderNode', + parameters=[{ + 'output_field_size': output_field_size, + 'cuboid_scaling_factor': cuboid_scaling_factor, + 'score_threshold': score_threshold, + 'object_name': object_name, + }] + ) + + centerpose_visualizer_node = ComposableNode( + name='centerpose_visualizer_node', + package='isaac_ros_centerpose', + plugin='nvidia::isaac_ros::centerpose::CenterPoseVisualizerNode', + parameters=[{ + 'show_axes': show_axes, + 'bounding_box_color': bounding_box_color, + }] + ) + rclcpp_container = ComposableNodeContainer( name='centerpose_container', namespace='', package='rclcpp_components', executable='component_container_mt', composable_node_descriptions=[ - centerpose_encoder_node, centerpose_inference_node], + centerpose_encoder_node, centerpose_inference_node, + centerpose_decoder_node, centerpose_visualizer_node], output='screen', ) - dope_decoder_node = Node( - name='centerpose_decoder_node', - package='isaac_ros_centerpose', - executable='CenterPoseDecoder', - parameters=[config], - output='screen' - ) - - final_launch_container = launch_args + \ - [rclcpp_container] + [dope_decoder_node] + final_launch_container = launch_args + [rclcpp_container] return LaunchDescription(final_launch_container) diff --git a/isaac_ros_centerpose/package.xml b/isaac_ros_centerpose/package.xml index 508093b..cfbf9b2 100644 --- a/isaac_ros_centerpose/package.xml +++ b/isaac_ros_centerpose/package.xml @@ -1,7 +1,7 @@