-
Notifications
You must be signed in to change notification settings - Fork 22
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
1 parent
d2027fd
commit b876656
Showing
21 changed files
with
526 additions
and
101 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Empty file.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,73 @@ | ||
| # 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_fiducials LANGUAGES C CXX) | ||
|
|
||
| if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") | ||
| add_compile_options(-fPIC -w) | ||
| endif() | ||
|
|
||
| # Dependencies | ||
| find_package(Eigen3 3.3 REQUIRED NO_MODULE) | ||
| find_package(isaac_ros_nitros_april_tag_detection_array_type REQUIRED) | ||
| find_package(GXF ${ISAAC_ROS_GXF_VERSION} MODULE REQUIRED | ||
| COMPONENTS | ||
| core | ||
| cuda | ||
| multimedia | ||
| serialization | ||
| std | ||
| ) | ||
| include(YamlCpp) | ||
|
|
||
| # nvApriltag | ||
| add_library(nvapriltags STATIC IMPORTED) | ||
|
|
||
| execute_process(COMMAND uname -m COMMAND tr -d '\n' OUTPUT_VARIABLE ARCHITECTURE) | ||
| message( STATUS "Architecture: ${ARCHITECTURE}" ) | ||
| if( ${ARCHITECTURE} STREQUAL "x86_64" ) | ||
| set(ARCH_GXF_PATH "lib_x86_64_cuda_11_8") | ||
| elseif( ${ARCHITECTURE} STREQUAL "aarch64" ) | ||
| set(ARCH_GXF_PATH "lib_aarch64_jetpack51") | ||
| endif() | ||
| set_property(TARGET nvapriltags PROPERTY IMPORTED_LOCATION ${NVAPRILTAGS}/${ARCH_GXF_PATH}/libapril_tagging.a) | ||
|
|
||
| # Fiducials extension | ||
| add_library(gxf_fiducials SHARED | ||
| components/cuda_april_tag_detector.cpp | ||
| fiducials.cpp | ||
| ) | ||
| set(CMAKE_INCLUDE_CURRENT_DIR TRUE) | ||
| target_include_directories(gxf_fiducials | ||
| PUBLIC | ||
| include | ||
| PRIVATE | ||
| ${NVAPRILTAGS}/nvapriltags | ||
| ${isaac_ros_nitros_april_tag_detection_array_type_INCLUDE_DIRS} | ||
| ) | ||
| target_link_libraries(gxf_fiducials | ||
| PRIVATE | ||
| Eigen3::Eigen | ||
| ${isaac_ros_nitros_april_tag_detection_array_type_LIBRARIES} | ||
| nvapriltags | ||
| PUBLIC | ||
| GXF::cuda | ||
| GXF::multimedia | ||
| GXF::serialization | ||
| GXF::std | ||
| yaml-cpp | ||
| ) |
249 changes: 249 additions & 0 deletions
249
isaac_ros_apriltag/gxf/fiducials/components/cuda_april_tag_detector.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,249 @@ | ||
| // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES | ||
| // Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
| // | ||
| // SPDX-License-Identifier: Apache-2.0 | ||
| #include "extensions/fiducials/components/cuda_april_tag_detector.hpp" | ||
|
|
||
| #include <cuda_runtime.h> | ||
| #include <string> | ||
| #include <utility> | ||
| #include <vector> | ||
|
|
||
| #include "nvAprilTags.h" | ||
| #include "engine/core/image/image.hpp" | ||
| #include "engine/gems/image/utils.hpp" | ||
| #include "extensions/fiducials/messages/fiducial_message.hpp" | ||
| #include "gxf/multimedia/video.hpp" | ||
| #include "gxf/std/timestamp.hpp" | ||
|
|
||
| namespace nvidia { | ||
| namespace isaac { | ||
|
|
||
| namespace { | ||
|
|
||
| // Gets corner points as a tensor | ||
| gxf::Expected<gxf::Tensor> CornersToTensor(const float2 corners[4], | ||
| gxf::Handle<gxf::Allocator> allocator) { | ||
| gxf::Tensor tensor; | ||
| return tensor.reshape<double>(gxf::Shape{4, 2}, gxf::MemoryStorageType::kHost, allocator) | ||
| .and_then([&]() { return tensor.data<double>(); }) | ||
| .map([&](double* points) { | ||
| const int stride = tensor.shape().dimension(0); | ||
| for (int i = 0; i < stride; i++) { | ||
| points[i] = corners[i].y; | ||
| points[i + stride] = corners[i].x; | ||
| } | ||
| return std::move(tensor); | ||
| }); | ||
| } | ||
|
|
||
| // Converts pose from april tag detection library to Pose3 | ||
| ::isaac::Pose3d DetectionToPose3d(const float translation[3], const float rot_flat[9]) { | ||
| // Rotation matrix from nvAprilTags is column major | ||
| ::isaac::Matrix3d rot_matrix; | ||
| rot_matrix << rot_flat[0], rot_flat[3], rot_flat[6], | ||
| rot_flat[1], rot_flat[4], rot_flat[7], | ||
| rot_flat[2], rot_flat[5], rot_flat[8]; | ||
| return ::isaac::Pose3d{ | ||
| ::isaac::SO3d::FromQuaternion(::isaac::Quaterniond(rot_matrix)), | ||
| ::isaac::Vector3d(translation[0], translation[1], translation[2]) | ||
| }; | ||
| } | ||
|
|
||
| } // namespace | ||
|
|
||
| struct CudaAprilTagDetector::AprilTagData { | ||
| // Handle used to interface with the stereo library. | ||
| nvAprilTagsHandle april_tags_handle; | ||
| // CUDA buffers to store the input image. | ||
| nvAprilTagsImageInput_t input_image; | ||
| // Camera intrinsics | ||
| nvAprilTagsCameraIntrinsics_t cam_intrinsics; | ||
| // Output vector of detected Tags | ||
| std::vector<nvAprilTagsID_t> tags; | ||
| // CUDA stream | ||
| cudaStream_t main_stream; | ||
| }; | ||
|
|
||
| CudaAprilTagDetector::CudaAprilTagDetector() {} | ||
|
|
||
| CudaAprilTagDetector::~CudaAprilTagDetector() {} | ||
|
|
||
| gxf_result_t CudaAprilTagDetector::registerInterface(gxf::Registrar* registrar) { | ||
| if (!registrar) { return GXF_ARGUMENT_NULL; } | ||
| gxf::Expected<void> result; | ||
| result &= registrar->parameter( | ||
| camera_image_, "camera_image", "Camera Image", | ||
| "Undistorted RGB camera image with intrinsics"); | ||
| result &= registrar->parameter( | ||
| april_tags_, "april_tags", "April Tags", | ||
| "AprilTag fiducial"); | ||
| result &= registrar->parameter( | ||
| allocator_, "allocator", "Allocator", | ||
| "Memory allocator for keypoints tensor"); | ||
| result &= registrar->parameter( | ||
| max_tags_, "max_tags", "Max Tags", | ||
| "Maximum number of AprilTags that can be detected", | ||
| 50); | ||
| result &= registrar->parameter( | ||
| tag_dimensions_, "tag_dimensions", "Tag Dimensions", | ||
| "AprilTag tag dimensions", | ||
| 0.18); | ||
| result &= registrar->parameter( | ||
| tag_family_, "tag_family", "Tag Family", | ||
| "AprilTag tag family", | ||
| std::string("tag36h11")); | ||
| result &= registrar->parameter( | ||
| video_buffer_name_, "video_buffer_name", "Video Buffer Name", | ||
| "Name of the VideoBuffer component to use", | ||
| gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); | ||
| result &= registrar->parameter( | ||
| camera_model_name_, "camera_model_name", "Camera Model Name", | ||
| "Name of the CameraModel component to use", | ||
| gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL); | ||
| return gxf::ToResultCode(result); | ||
| } | ||
|
|
||
| gxf_result_t CudaAprilTagDetector::initialize() { | ||
| if (tag_family_.get() != "tag36h11") { | ||
| GXF_LOG_ERROR("CudaAprilTagDetector only supports tag36h11 AprilTags"); | ||
| return GXF_PARAMETER_OUT_OF_RANGE; | ||
| } | ||
| impl_ = MakeUniqueNoThrow<AprilTagData>(); | ||
| return impl_ != nullptr ? GXF_SUCCESS : GXF_OUT_OF_MEMORY; | ||
| } | ||
|
|
||
| gxf_result_t CudaAprilTagDetector::deinitialize() { | ||
| impl_ = nullptr; | ||
| return GXF_SUCCESS; | ||
| } | ||
|
|
||
| gxf_result_t CudaAprilTagDetector::tick() { | ||
| // Receive message entity | ||
| auto entity = camera_image_->receive(); | ||
| if (!entity) { | ||
| return gxf::ToResultCode(entity); | ||
| } | ||
|
|
||
| // Get frame | ||
| auto video_buffer_name = video_buffer_name_.try_get(); | ||
| const char* frame_name = video_buffer_name ? video_buffer_name->c_str() : nullptr; | ||
| auto frame = entity->get<gxf::VideoBuffer>(frame_name); | ||
| if (!frame) { | ||
| return gxf::ToResultCode(frame); | ||
| } | ||
|
|
||
| // Get intrinsics | ||
| auto camera_model_name = camera_model_name_.try_get(); | ||
| const char* intrinsics_name = camera_model_name ? camera_model_name->c_str() : nullptr; | ||
| auto intrinsics = entity->get<gxf::CameraModel>(intrinsics_name); | ||
| if (!intrinsics) { | ||
| return gxf::ToResultCode(intrinsics); | ||
| } | ||
|
|
||
| if (impl_->april_tags_handle == nullptr) { | ||
| // Initalize AprilTag library parameters | ||
| auto result = createAprilTagDetector(intrinsics.value()); | ||
| if (!result) { | ||
| return gxf::ToResultCode(result); | ||
| } | ||
| } | ||
|
|
||
| if (frame.value()->video_frame_info().color_format != | ||
| gxf::VideoFormat::GXF_VIDEO_FORMAT_RGB) { | ||
| GXF_LOG_ERROR("CudaAprilTagDetector only supports RGB images"); | ||
| return GXF_FAILURE; | ||
| } | ||
|
|
||
| if (frame.value()->storage_type() != gxf::MemoryStorageType::kDevice) { | ||
| GXF_LOG_ERROR("CudaAprilTagDetector only supports CUDA images"); | ||
| return GXF_FAILURE; | ||
| } | ||
|
|
||
| // Avoid memory copy by running detection on image view | ||
| impl_->input_image.width = frame.value()->video_frame_info().width; | ||
| impl_->input_image.height = frame.value()->video_frame_info().height; | ||
| impl_->input_image.pitch = frame.value()->video_frame_info().color_planes[0].stride; | ||
| impl_->input_image.dev_ptr = reinterpret_cast<uchar3*>(frame.value()->pointer()); | ||
|
|
||
| // Run AprilTags detection | ||
| uint32_t num_tags; | ||
| const int error = nvAprilTagsDetect(impl_->april_tags_handle, &(impl_->input_image), | ||
| impl_->tags.data(), &num_tags, max_tags_, impl_->main_stream); | ||
| if (error != 0) { | ||
| GXF_LOG_ERROR("Call to nvAprilTagsDetect failed with error code %d", error); | ||
| return GXF_FAILURE; | ||
| } | ||
|
|
||
| return gxf::ToResultCode( | ||
| CreateFiducialListMessage(context(), num_tags) | ||
| .map([&](FiducialListMessageParts message) -> gxf::Expected<void> { | ||
| for (uint32_t i = 0; i < num_tags; i++) { | ||
| const nvAprilTagsID_t& tag = impl_->tags[i]; | ||
| message.info[i].value()->type = FiducialInfo::Type::kAprilTag; | ||
| message.info[i].value()->id = tag_family_.get() + "_" + std::to_string(tag.id); | ||
| auto keypoints = CornersToTensor(tag.corners, allocator_); | ||
| if (!keypoints) { | ||
| return gxf::ForwardError(keypoints); | ||
| } | ||
| *message.keypoints[i].value() = std::move(keypoints.value()); | ||
| *message.pose[i].value() = DetectionToPose3d(tag.translation, tag.orientation); | ||
| } | ||
| return april_tags_->publish(message.entity); | ||
| })); | ||
| } | ||
|
|
||
| gxf_result_t CudaAprilTagDetector::stop() { | ||
| if (impl_->april_tags_handle != nullptr) { | ||
| cudaStreamDestroy(impl_->main_stream); | ||
| nvAprilTagsDestroy(impl_->april_tags_handle); | ||
| } | ||
| return GXF_SUCCESS; | ||
| } | ||
|
|
||
| gxf::Expected<void> CudaAprilTagDetector::createAprilTagDetector( | ||
| gxf::Handle<gxf::CameraModel> intrinsics) { | ||
| if (!intrinsics) { | ||
| return gxf::Unexpected{GXF_ARGUMENT_NULL}; | ||
| } | ||
|
|
||
| // Get camera intrinsics | ||
| const float fx = intrinsics->focal_length.x; | ||
| const float fy = intrinsics->focal_length.y; | ||
| const float cx = intrinsics->principal_point.x; | ||
| const float cy = intrinsics->principal_point.y; | ||
| impl_->cam_intrinsics = {fx, fy, cx, cy}; | ||
|
|
||
| // Create AprilTags detector instance and get handle | ||
| const int error = nvCreateAprilTagsDetector(&(impl_->april_tags_handle), | ||
| intrinsics->dimensions.x, intrinsics->dimensions.y, | ||
| nvAprilTagsFamily::NVAT_TAG36H11, | ||
| &(impl_->cam_intrinsics), tag_dimensions_); | ||
| if (error != 0) { | ||
| GXF_LOG_ERROR("Call to nvCreateAprilTagsDetector failed with error code %d", error); | ||
| return gxf::Unexpected{GXF_FAILURE}; | ||
| } | ||
|
|
||
| // Create stream for detection | ||
| cudaStreamCreate(&(impl_->main_stream)); | ||
|
|
||
| // Allocate the output vector to contain detected AprilTags. | ||
| impl_->tags.resize(max_tags_); | ||
|
|
||
| return gxf::Success; | ||
| } | ||
|
|
||
| } // namespace isaac | ||
| } // namespace nvidia |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,35 @@ | ||
| // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES | ||
| // Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
| // | ||
| // SPDX-License-Identifier: Apache-2.0 | ||
| #include "extensions/fiducials/components/cuda_april_tag_detector.hpp" | ||
| #include "extensions/fiducials/gems/fiducial_info.hpp" | ||
| #include "gxf/std/extension_factory_helper.hpp" | ||
|
|
||
| GXF_EXT_FACTORY_BEGIN() | ||
|
|
||
| GXF_EXT_FACTORY_SET_INFO(0xd8d7816ec0485ad4, 0xff795a414bd445ca, "Fiducials", | ||
| "Extension containing components for working with fiducials", | ||
| "Isaac SDK", "1.0.0", "LICENSE"); | ||
|
|
||
| GXF_EXT_FACTORY_ADD_0(0xe91d3fa6a42b85ff, 0x966f4e80c607ca9e, | ||
| nvidia::isaac::FiducialInfo, | ||
| "Holds fiducial meta information."); | ||
|
|
||
| GXF_EXT_FACTORY_ADD(0xeacaaee8b4f923b8, 0xc5c90679ce99113c, | ||
| nvidia::isaac::CudaAprilTagDetector, nvidia::gxf::Codelet, | ||
| "Detects AprilTags in images with nvAprilTags library."); | ||
|
|
||
| GXF_EXT_FACTORY_END() |
66 changes: 66 additions & 0 deletions
66
...priltag/gxf/fiducials/include/extensions/fiducials/components/cuda_april_tag_detector.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,66 @@ | ||
| // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES | ||
| // Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
| // | ||
| // SPDX-License-Identifier: Apache-2.0 | ||
| #pragma once | ||
|
|
||
| #include <memory> | ||
| #include <string> | ||
|
|
||
| #include "gxf/multimedia/camera.hpp" | ||
| #include "gxf/std/allocator.hpp" | ||
| #include "gxf/std/codelet.hpp" | ||
| #include "gxf/std/receiver.hpp" | ||
| #include "gxf/std/transmitter.hpp" | ||
|
|
||
| namespace nvidia { | ||
| namespace isaac { | ||
|
|
||
| // Detects AprilTags in images with nvAprilTags library | ||
| class CudaAprilTagDetector : public gxf::Codelet { | ||
| public: | ||
| // Explicitly declare constructors and destructors | ||
| // to get around forward declaration of AprilTagsData | ||
| CudaAprilTagDetector(); | ||
| ~CudaAprilTagDetector(); | ||
|
|
||
| gxf_result_t registerInterface(gxf::Registrar* registrar) override; | ||
| gxf_result_t initialize() override; | ||
| gxf_result_t deinitialize() override; | ||
|
|
||
| gxf_result_t start() override { return GXF_SUCCESS; } | ||
| gxf_result_t tick() override; | ||
| gxf_result_t stop() override; | ||
|
|
||
| private: | ||
| // Initializes AprilTag detector with camera intrinsics | ||
| gxf::Expected<void> createAprilTagDetector(gxf::Handle<gxf::CameraModel> intrinsics); | ||
|
|
||
| gxf::Parameter<gxf::Handle<gxf::Receiver>> camera_image_; | ||
| gxf::Parameter<gxf::Handle<gxf::Transmitter>> april_tags_; | ||
| gxf::Parameter<gxf::Handle<gxf::Allocator>> allocator_; | ||
| gxf::Parameter<int> max_tags_; | ||
| gxf::Parameter<double> tag_dimensions_; | ||
| gxf::Parameter<std::string> tag_family_; | ||
| gxf::Parameter<std::string> video_buffer_name_; | ||
| gxf::Parameter<std::string> camera_model_name_; | ||
|
|
||
| // Hide the AprilTagData implementation details | ||
| struct AprilTagData; | ||
| std::unique_ptr<AprilTagData> impl_; | ||
| }; | ||
|
|
||
| } // namespace isaac | ||
| } // namespace nvidia |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file was deleted.
Oops, something went wrong.
This file was deleted.
Oops, something went wrong.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Git LFS file not shown
Git LFS file not shown
Git LFS file not shown