├── .clang-format ├── .github ├── deps.repos └── workflows │ └── ci.yml ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cfg └── tags_36h11.yaml ├── launch └── camera_36h11.launch.yml ├── package.xml └── src ├── AprilTagNode.cpp ├── conversion.cpp ├── pose_estimation.cpp ├── pose_estimation.hpp ├── tag_functions.cpp └── tag_functions.hpp /.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: LLVM 2 | AccessModifierOffset: -4 3 | AlignAfterOpenBracket: Align 4 | AlignConsecutiveAssignments: None 5 | AlignOperands: Align 6 | AllowAllArgumentsOnNextLine: false 7 | AllowAllConstructorInitializersOnNextLine: false 8 | AllowAllParametersOfDeclarationOnNextLine: false 9 | AllowShortBlocksOnASingleLine: Always 10 | AllowShortCaseLabelsOnASingleLine: false 11 | AllowShortFunctionsOnASingleLine: All 12 | AllowShortIfStatementsOnASingleLine: WithoutElse 13 | AllowShortLambdasOnASingleLine: All 14 | AllowShortLoopsOnASingleLine: true 15 | AlwaysBreakAfterReturnType: None 16 | AlwaysBreakTemplateDeclarations: Yes 17 | BreakBeforeBraces: Custom 18 | BraceWrapping: 19 | AfterCaseLabel: true 20 | AfterClass: false 21 | AfterControlStatement: Never 22 | AfterEnum: true 23 | AfterFunction: true 24 | AfterNamespace: true 25 | AfterStruct: true 26 | AfterUnion: true 27 | AfterExternBlock: true 28 | BeforeCatch: true 29 | BeforeElse: true 30 | IndentBraces: false 31 | SplitEmptyFunction: false 32 | SplitEmptyRecord: true 33 | BreakBeforeBinaryOperators: None 34 | BreakBeforeTernaryOperators: true 35 | BreakConstructorInitializers: BeforeColon 36 | BreakInheritanceList: BeforeColon 37 | ColumnLimit: 0 38 | CompactNamespaces: false 39 | ConstructorInitializerIndentWidth: 2 40 | ContinuationIndentWidth: 4 41 | IndentCaseLabels: true 42 | IndentPPDirectives: None 43 | IndentWidth: 4 44 | KeepEmptyLinesAtTheStartOfBlocks: true 45 | MaxEmptyLinesToKeep: 2 46 | NamespaceIndentation: All 47 | PointerAlignment: Left 48 | ReflowComments: false 49 | SpaceAfterCStyleCast: true 50 | SpaceAfterLogicalNot: false 51 | SpaceAfterTemplateKeyword: false 52 | SpaceBeforeAssignmentOperators: true 53 | SpaceBeforeCpp11BracedList: false 54 | SpaceBeforeCtorInitializerColon: true 55 | SpaceBeforeInheritanceColon: true 56 | SpaceBeforeParens: Never 57 | SpaceBeforeRangeBasedForLoopColon: true 58 | SpaceInEmptyParentheses: false 59 | SpacesBeforeTrailingComments: 0 60 | SpacesInAngles: false 61 | SpacesInCStyleCastParentheses: false 62 | SpacesInContainerLiterals: false 63 | SpacesInParentheses: false 64 | SpacesInSquareBrackets: false 65 | TabWidth: 4 66 | UseTab: Never 67 | -------------------------------------------------------------------------------- /.github/deps.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | src/apriltag: 3 | type: git 4 | url: https://github.com/AprilRobotics/apriltag.git 5 | version: v3.4.2 6 | src/apriltag_msgs: 7 | type: git 8 | url: https://github.com/christianrauch/apriltag_msgs.git 9 | version: 2.0.1 10 | -------------------------------------------------------------------------------- /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | build: 7 | name: apriltag_ros 8 | runs-on: ubuntu-latest 9 | 10 | strategy: 11 | matrix: 12 | include: 13 | - {image: "ubuntu:22.04", ros: humble} 14 | - {image: "ubuntu:24.04", ros: jazzy} 15 | - {image: "almalinux:8", ros: humble} 16 | - {image: "almalinux:9", ros: jazzy} 17 | 18 | container: 19 | image: ${{ matrix.image }} 20 | 21 | steps: 22 | - uses: actions/checkout@v4 23 | 24 | - uses: ros-tooling/setup-ros@v0.7 25 | 26 | - uses: ros-tooling/action-ros-ci@v0.4 27 | with: 28 | package-name: apriltag_ros 29 | target-ros2-distro: ${{ matrix.ros }} 30 | vcs-repo-file-url: .github/deps.repos 31 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(apriltag_ros) 4 | 5 | set(CMAKE_CXX_STANDARD 14) 6 | 7 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 8 | add_compile_options(-Werror -Wall -Wextra -Wpedantic) 9 | add_link_options("-Wl,-z,relro,-z,now,-z,defs") 10 | endif() 11 | 12 | option(ASAN "use AddressSanitizer to detect memory issues" OFF) 13 | 14 | if(ASAN) 15 | set(ASAN_FLAGS "\ 16 | -fsanitize=address \ 17 | -fsanitize=bool \ 18 | -fsanitize=bounds \ 19 | -fsanitize=enum \ 20 | -fsanitize=float-cast-overflow \ 21 | -fsanitize=float-divide-by-zero \ 22 | -fsanitize=nonnull-attribute \ 23 | -fsanitize=returns-nonnull-attribute \ 24 | -fsanitize=signed-integer-overflow \ 25 | -fsanitize=undefined \ 26 | -fsanitize=vla-bound \ 27 | -fno-sanitize=alignment \ 28 | -fsanitize=leak \ 29 | -fsanitize=object-size \ 30 | ") 31 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${ASAN_FLAGS}") 32 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ASAN_FLAGS}") 33 | endif() 34 | 35 | find_package(rclcpp REQUIRED) 36 | find_package(rclcpp_components REQUIRED) 37 | find_package(sensor_msgs REQUIRED) 38 | find_package(apriltag_msgs REQUIRED) 39 | find_package(tf2_ros REQUIRED) 40 | find_package(image_transport REQUIRED) 41 | find_package(cv_bridge REQUIRED) 42 | find_package(Eigen3 REQUIRED NO_MODULE) 43 | find_package(Threads REQUIRED) 44 | find_package(OpenCV REQUIRED COMPONENTS core calib3d) 45 | find_package(apriltag 3.2 REQUIRED) 46 | 47 | if(cv_bridge_VERSION VERSION_GREATER_EQUAL 3.3.0) 48 | add_compile_definitions(cv_bridge_HPP) 49 | endif() 50 | 51 | 52 | # database of tag functions 53 | add_library(tags STATIC 54 | src/tag_functions.cpp 55 | ) 56 | target_link_libraries(tags 57 | apriltag::apriltag 58 | ) 59 | set_property(TARGET tags PROPERTY 60 | POSITION_INDEPENDENT_CODE ON 61 | ) 62 | 63 | 64 | # conversion functions as template specialisation 65 | add_library(conversion STATIC 66 | src/conversion.cpp 67 | ) 68 | target_link_libraries(conversion 69 | apriltag::apriltag 70 | Eigen3::Eigen 71 | opencv_core 72 | ) 73 | ament_target_dependencies(conversion 74 | geometry_msgs 75 | tf2 76 | ) 77 | set_property(TARGET conversion PROPERTY 78 | POSITION_INDEPENDENT_CODE ON 79 | ) 80 | 81 | 82 | # pose estimation methods 83 | add_library(pose_estimation STATIC 84 | src/pose_estimation.cpp 85 | ) 86 | target_link_libraries(pose_estimation 87 | apriltag::apriltag 88 | Eigen3::Eigen 89 | opencv_calib3d 90 | conversion 91 | ) 92 | ament_target_dependencies(pose_estimation 93 | geometry_msgs 94 | tf2 95 | ) 96 | set_property(TARGET pose_estimation PROPERTY 97 | POSITION_INDEPENDENT_CODE ON 98 | ) 99 | 100 | 101 | # composable node 102 | add_library(AprilTagNode SHARED 103 | src/AprilTagNode.cpp 104 | ) 105 | ament_target_dependencies(AprilTagNode 106 | rclcpp 107 | rclcpp_components 108 | sensor_msgs 109 | apriltag_msgs 110 | tf2_ros 111 | image_transport 112 | cv_bridge 113 | ) 114 | target_link_libraries(AprilTagNode 115 | apriltag::apriltag 116 | tags 117 | pose_estimation 118 | ) 119 | rclcpp_components_register_node(AprilTagNode 120 | PLUGIN "AprilTagNode" 121 | EXECUTABLE "apriltag_node" 122 | ) 123 | 124 | ament_environment_hooks(${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH}) 125 | 126 | install(TARGETS AprilTagNode 127 | ARCHIVE DESTINATION lib 128 | LIBRARY DESTINATION lib 129 | ) 130 | 131 | install(DIRECTORY cfg 132 | DESTINATION share/${PROJECT_NAME} 133 | ) 134 | 135 | install(DIRECTORY launch 136 | DESTINATION share/${PROJECT_NAME} 137 | ) 138 | 139 | if(BUILD_TESTING) 140 | set(ament_cmake_clang_format_CONFIG_FILE "${CMAKE_SOURCE_DIR}/.clang-format") 141 | find_package(ament_lint_auto REQUIRED) 142 | # the following line skips the linter which checks for copyrights 143 | # comment the line when a copyright and license is added to all source files 144 | set(ament_cmake_copyright_FOUND TRUE) 145 | # the following line skips cpplint (only works in a git repo) 146 | # comment the line when this package is in a git repo and when 147 | # a copyright and license is added to all source files 148 | set(ament_cmake_cpplint_FOUND TRUE) 149 | ament_lint_auto_find_test_dependencies() 150 | endif() 151 | 152 | ament_package() 153 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 Christian Rauch 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # AprilTag ROS 2 Node 2 | 3 | This ROS 2 node uses the AprilTag library to detect AprilTags in images and publish their pose, id and additional metadata. 4 | 5 | For more information on AprilTag, the paper and the reference implementation: https://april.eecs.umich.edu/software/apriltag.html 6 | 7 | ## Topics 8 | 9 | ### Subscriptions: 10 | The node subscribes via a `image_transport::CameraSubscriber` to rectified images on topic `image_rect`. The set of topic names depends on the type of image transport (parameter `image_transport`) selected (`raw` or `compressed`): 11 | - `image_rect` (`raw`, type: `sensor_msgs/msg/Image`) 12 | - `image_rect/compressed` (`compressed`, type: `sensor_msgs/msg/CompressedImage`) 13 | - `camera_info` (type: `sensor_msgs/msg/CameraInfo`) 14 | 15 | ### Publisher: 16 | - `/tf` (type: `tf2_msgs/msg/TFMessage`) 17 | - `detections` (type: `apriltag_msgs/msg/AprilTagDetectionArray`) 18 | 19 | The camera intrinsics `P` in `CameraInfo` are used to compute the marker tag pose `T` from the homography `H`. The image and the camera intrinsics need to have the same timestamp. 20 | 21 | The tag poses are published on the standard TF topic `/tf` with the header set to the image header and `child_frame_id` set to either `tag:` (e.g. "tag36h11:0") or the frame name selected via configuration file. Additional information about detected tags is published as `AprilTagDetectionArray` message, which contains the original homography matrix, the `hamming` distance and the `decision_margin` of the detection. 22 | 23 | ## Configuration 24 | 25 | The node is configured via a yaml configurations file. For the complete ROS yaml parameter file syntax, see: https://github.com/ros2/rcl/tree/master/rcl_yaml_param_parser. 26 | 27 | The configuration file has the format: 28 | ```yaml 29 | apriltag: # node name 30 | ros__parameters: 31 | # setup (defaults) 32 | image_transport: raw # image format: "raw" or "compressed" 33 | family: 36h11 # tag family name: 16h5, 25h9, 36h11 34 | size: 1.0 # default tag edge size in meter 35 | profile: false # print profiling information to stdout 36 | 37 | # tuning of detection (defaults) 38 | max_hamming: 0 # maximum allowed hamming distance (corrected bits) 39 | detector: 40 | threads: 1 # number of threads 41 | decimate: 2.0 # decimate resolution for quad detection 42 | blur: 0.0 # sigma of Gaussian blur for quad detection 43 | refine: 1 # snap to strong gradients 44 | sharpening: 0.25 # sharpening of decoded images 45 | debug: 0 # write additional debugging images to current working directory 46 | 47 | pose_estimation_method: "pnp" # method for estimating the tag pose 48 | 49 | # (optional) list of tags 50 | # If defined, 'frames' and 'sizes' must have the same length as 'ids'. 51 | tag: 52 | ids: [, , ...] # tag IDs for which to publish transform 53 | frames: [, , ...] # frame names 54 | sizes: [, , ...] # tag-specific edge size, overrides the default 'size' 55 | ``` 56 | 57 | The `family` (string) defines the tag family for the detector and must be one of `16h5`, `25h9`, `36h11`, `Circle21h7`, `Circle49h12`, `Custom48h12`, `Standard41h12`, `Standard52h13`. `size` (float) is the tag edge size in meters, assuming square markers. 58 | 59 | Instead of publishing all tag poses, the list `tag.ids` can be used to only publish selected tag IDs. Each tag can have an associated child frame name in `tag.frames` and a tag specific size in `tag.sizes`. These lists must either have the same length as `tag.ids` or may be empty. In this case, a default frame name of the form `tag:` and the default tag edge size `size` will be used. 60 | 61 | The remaining parameters are set to the their default values from the library. See `apriltag.h` for a more detailed description of their function. 62 | 63 | See [tags_36h11.yaml](cfg/tags_36h11.yaml) for an example configuration that publishes specific tag poses of the 36h11 family. 64 | 65 | ## Nodes 66 | 67 | ### Standalone Executable 68 | 69 | The `apriltag_node` executable can be launched with topic remappings and a configuration file: 70 | ```sh 71 | ros2 run apriltag_ros apriltag_node --ros-args \ 72 | -r image_rect:=/camera/image \ 73 | -r camera_info:=/camera/camera_info \ 74 | --params-file `ros2 pkg prefix apriltag_ros`/share/apriltag_ros/cfg/tags_36h11.yaml 75 | ``` 76 | 77 | ### Composable Node 78 | 79 | For more efficient intraprocess communication, a composable node is provided: 80 | ```sh 81 | $ ros2 component types 82 | apriltag_ros 83 | AprilTagNode 84 | ``` 85 | 86 | This `AprilTagNode` component can be loaded with other nodes into a "container node" process where they used shared-memory communication to prevent unnecessary data copies. The example launch file [camera_36h11.launch.yml](launch/camera_36h11.launch.yml) loads the `AprilTagNode` component together with the `camera::CameraNode` component from the [`camera_ros` package](https://index.ros.org/p/camera_ros/) (`sudo apt install ros-$ROS_DISTRO-camera-ros`) into one container and enables `use_intra_process_comms` for both: 87 | ```sh 88 | ros2 launch apriltag_ros camera_36h11.launch.yml 89 | ``` 90 | -------------------------------------------------------------------------------- /cfg/tags_36h11.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | image_transport: raw # image format 4 | family: 36h11 # tag family name 5 | size: 0.173 # tag edge size in meter 6 | max_hamming: 0 # maximum allowed hamming distance (corrected bits) 7 | 8 | # see "apriltag.h" 'struct apriltag_detector' for more documentation on these optional parameters 9 | detector: 10 | threads: 1 # number of threads 11 | decimate: 2.0 # decimate resolution for quad detection 12 | blur: 0.0 # sigma of Gaussian blur for quad detection 13 | refine: True # snap to strong gradients 14 | sharpening: 0.25 # sharpening of decoded images 15 | debug: False # write additional debugging images to current working directory 16 | 17 | 18 | pose_estimation_method: "pnp" # method for estimating the tag pose 19 | 20 | # optional list of tags 21 | tag: 22 | ids: [9, 14] # tag ID 23 | frames: [base, object] # optional frame name 24 | sizes: [0.162, 0.162] # optional tag-specific edge size 25 | -------------------------------------------------------------------------------- /launch/camera_36h11.launch.yml: -------------------------------------------------------------------------------- 1 | launch: 2 | - arg: 3 | name: device 4 | default: "0" 5 | 6 | - node_container: 7 | pkg: rclcpp_components 8 | exec: component_container 9 | name: apriltag_container 10 | namespace: "" 11 | composable_node: 12 | - pkg: camera_ros 13 | plugin: camera::CameraNode 14 | name: camera 15 | namespace: camera 16 | param: 17 | - name: camera 18 | value: $(var device) 19 | extra_arg: 20 | - name: use_intra_process_comms 21 | value: "True" 22 | 23 | - pkg: image_proc 24 | plugin: image_proc::RectifyNode 25 | name: rectify 26 | namespace: camera 27 | remap: 28 | - from: image 29 | to: /camera/camera/image_raw 30 | - from: camera_info 31 | to: /camera/camera/camera_info 32 | extra_arg: 33 | - name: use_intra_process_comms 34 | value: "True" 35 | 36 | - pkg: apriltag_ros 37 | plugin: AprilTagNode 38 | name: apriltag 39 | namespace: apriltag 40 | remap: 41 | - from: /apriltag/image_rect 42 | to: /camera/image_rect 43 | - from: /camera/camera_info 44 | to: /camera/camera/camera_info 45 | param: 46 | - from: $(find-pkg-share apriltag_ros)/cfg/tags_36h11.yaml 47 | extra_arg: 48 | - name: use_intra_process_comms 49 | value: "True" 50 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | apriltag_ros 5 | 3.2.2 6 | AprilTag detection node 7 | Christian Rauch 8 | MIT 9 | 10 | ament_cmake 11 | 12 | eigen 13 | 14 | rclcpp 15 | rclcpp_components 16 | sensor_msgs 17 | tf2_ros 18 | apriltag_msgs 19 | apriltag 20 | image_transport 21 | cv_bridge 22 | 23 | camera_ros 24 | image_proc 25 | image_transport_plugins 26 | 27 | ament_lint_auto 28 | ament_cmake_clang_format 29 | clang-format 30 | ament_cmake_cppcheck 31 | 32 | 33 | ament_cmake 34 | 35 | 36 | -------------------------------------------------------------------------------- /src/AprilTagNode.cpp: -------------------------------------------------------------------------------- 1 | // ros 2 | #include "pose_estimation.hpp" 3 | #include 4 | #include 5 | #ifdef cv_bridge_HPP 6 | #include 7 | #else 8 | #include 9 | #endif 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | // apriltag 19 | #include "tag_functions.hpp" 20 | #include 21 | 22 | 23 | #define IF(N, V) \ 24 | if(assign_check(parameter, N, V)) continue; 25 | 26 | template 27 | void assign(const rclcpp::Parameter& parameter, T& var) 28 | { 29 | var = parameter.get_value(); 30 | } 31 | 32 | template 33 | void assign(const rclcpp::Parameter& parameter, std::atomic& var) 34 | { 35 | var = parameter.get_value(); 36 | } 37 | 38 | template 39 | bool assign_check(const rclcpp::Parameter& parameter, const std::string& name, T& var) 40 | { 41 | if(parameter.get_name() == name) { 42 | assign(parameter, var); 43 | return true; 44 | } 45 | return false; 46 | } 47 | 48 | rcl_interfaces::msg::ParameterDescriptor 49 | descr(const std::string& description, const bool& read_only = false) 50 | { 51 | rcl_interfaces::msg::ParameterDescriptor descr; 52 | 53 | descr.description = description; 54 | descr.read_only = read_only; 55 | 56 | return descr; 57 | } 58 | 59 | class AprilTagNode : public rclcpp::Node { 60 | public: 61 | AprilTagNode(const rclcpp::NodeOptions& options); 62 | 63 | ~AprilTagNode() override; 64 | 65 | private: 66 | const OnSetParametersCallbackHandle::SharedPtr cb_parameter; 67 | 68 | apriltag_family_t* tf; 69 | apriltag_detector_t* const td; 70 | 71 | // parameter 72 | std::mutex mutex; 73 | double tag_edge_size; 74 | std::atomic max_hamming; 75 | std::atomic profile; 76 | std::unordered_map tag_frames; 77 | std::unordered_map tag_sizes; 78 | 79 | std::function tf_destructor; 80 | 81 | const image_transport::CameraSubscriber sub_cam; 82 | const rclcpp::Publisher::SharedPtr pub_detections; 83 | tf2_ros::TransformBroadcaster tf_broadcaster; 84 | 85 | pose_estimation_f estimate_pose = nullptr; 86 | 87 | void onCamera(const sensor_msgs::msg::Image::ConstSharedPtr& msg_img, const sensor_msgs::msg::CameraInfo::ConstSharedPtr& msg_ci); 88 | 89 | rcl_interfaces::msg::SetParametersResult onParameter(const std::vector& parameters); 90 | }; 91 | 92 | RCLCPP_COMPONENTS_REGISTER_NODE(AprilTagNode) 93 | 94 | 95 | AprilTagNode::AprilTagNode(const rclcpp::NodeOptions& options) 96 | : Node("apriltag", options), 97 | // parameter 98 | cb_parameter(add_on_set_parameters_callback(std::bind(&AprilTagNode::onParameter, this, std::placeholders::_1))), 99 | td(apriltag_detector_create()), 100 | // topics 101 | sub_cam(image_transport::create_camera_subscription( 102 | this, 103 | this->get_node_topics_interface()->resolve_topic_name("image_rect"), 104 | std::bind(&AprilTagNode::onCamera, this, std::placeholders::_1, std::placeholders::_2), 105 | declare_parameter("image_transport", "raw", descr({}, true)), 106 | rmw_qos_profile_sensor_data)), 107 | pub_detections(create_publisher("detections", rclcpp::QoS(1))), 108 | tf_broadcaster(this) 109 | { 110 | // read-only parameters 111 | const std::string tag_family = declare_parameter("family", "36h11", descr("tag family", true)); 112 | tag_edge_size = declare_parameter("size", 1.0, descr("default tag size", true)); 113 | 114 | // get tag names, IDs and sizes 115 | const auto ids = declare_parameter("tag.ids", std::vector{}, descr("tag ids", true)); 116 | const auto frames = declare_parameter("tag.frames", std::vector{}, descr("tag frame names per id", true)); 117 | const auto sizes = declare_parameter("tag.sizes", std::vector{}, descr("tag sizes per id", true)); 118 | 119 | // get method for estimating tag pose 120 | const std::string& pose_estimation_method = 121 | declare_parameter("pose_estimation_method", "pnp", 122 | descr("pose estimation method: \"pnp\" (more accurate) or \"homography\" (faster), " 123 | "set to \"\" (empty) to disable pose estimation", 124 | true)); 125 | 126 | if(!pose_estimation_method.empty()) { 127 | if(pose_estimation_methods.count(pose_estimation_method)) { 128 | estimate_pose = pose_estimation_methods.at(pose_estimation_method); 129 | } 130 | else { 131 | RCLCPP_ERROR_STREAM(get_logger(), "Unknown pose estimation method '" << pose_estimation_method << "'."); 132 | } 133 | } 134 | 135 | // detector parameters in "detector" namespace 136 | declare_parameter("detector.threads", td->nthreads, descr("number of threads")); 137 | declare_parameter("detector.decimate", td->quad_decimate, descr("decimate resolution for quad detection")); 138 | declare_parameter("detector.blur", td->quad_sigma, descr("sigma of Gaussian blur for quad detection")); 139 | declare_parameter("detector.refine", td->refine_edges, descr("snap to strong gradients")); 140 | declare_parameter("detector.sharpening", td->decode_sharpening, descr("sharpening of decoded images")); 141 | declare_parameter("detector.debug", td->debug, descr("write additional debugging images to working directory")); 142 | 143 | declare_parameter("max_hamming", 0, descr("reject detections with more corrected bits than allowed")); 144 | declare_parameter("profile", false, descr("print profiling information to stdout")); 145 | 146 | if(!frames.empty()) { 147 | if(ids.size() != frames.size()) { 148 | throw std::runtime_error("Number of tag ids (" + std::to_string(ids.size()) + ") and frames (" + std::to_string(frames.size()) + ") mismatch!"); 149 | } 150 | for(size_t i = 0; i < ids.size(); i++) { tag_frames[ids[i]] = frames[i]; } 151 | } 152 | 153 | if(!sizes.empty()) { 154 | // use tag specific size 155 | if(ids.size() != sizes.size()) { 156 | throw std::runtime_error("Number of tag ids (" + std::to_string(ids.size()) + ") and sizes (" + std::to_string(sizes.size()) + ") mismatch!"); 157 | } 158 | for(size_t i = 0; i < ids.size(); i++) { tag_sizes[ids[i]] = sizes[i]; } 159 | } 160 | 161 | if(tag_fun.count(tag_family)) { 162 | tf = tag_fun.at(tag_family).first(); 163 | tf_destructor = tag_fun.at(tag_family).second; 164 | apriltag_detector_add_family(td, tf); 165 | } 166 | else { 167 | throw std::runtime_error("Unsupported tag family: " + tag_family); 168 | } 169 | } 170 | 171 | AprilTagNode::~AprilTagNode() 172 | { 173 | apriltag_detector_destroy(td); 174 | tf_destructor(tf); 175 | } 176 | 177 | void AprilTagNode::onCamera(const sensor_msgs::msg::Image::ConstSharedPtr& msg_img, 178 | const sensor_msgs::msg::CameraInfo::ConstSharedPtr& msg_ci) 179 | { 180 | // camera intrinsics for rectified images 181 | const std::array intrinsics = {msg_ci->p[0], msg_ci->p[5], msg_ci->p[2], msg_ci->p[6]}; 182 | 183 | // check for valid intrinsics 184 | const bool calibrated = msg_ci->width && msg_ci->height && 185 | intrinsics[0] && intrinsics[1] && intrinsics[2] && intrinsics[3]; 186 | 187 | if(estimate_pose != nullptr && !calibrated) { 188 | RCLCPP_WARN_STREAM(get_logger(), "The camera is not calibrated! Set 'pose_estimation_method' to \"\" (empty) to disable pose estimation and this warning."); 189 | } 190 | 191 | // convert to 8bit monochrome image 192 | const cv::Mat img_uint8 = cv_bridge::toCvShare(msg_img, "mono8")->image; 193 | 194 | image_u8_t im{img_uint8.cols, img_uint8.rows, img_uint8.cols, img_uint8.data}; 195 | 196 | // detect tags 197 | mutex.lock(); 198 | zarray_t* detections = apriltag_detector_detect(td, &im); 199 | mutex.unlock(); 200 | 201 | if(profile) 202 | timeprofile_display(td->tp); 203 | 204 | apriltag_msgs::msg::AprilTagDetectionArray msg_detections; 205 | msg_detections.header = msg_img->header; 206 | 207 | std::vector tfs; 208 | 209 | for(int i = 0; i < zarray_size(detections); i++) { 210 | apriltag_detection_t* det; 211 | zarray_get(detections, i, &det); 212 | 213 | RCLCPP_DEBUG(get_logger(), 214 | "detection %3d: id (%2dx%2d)-%-4d, hamming %d, margin %8.3f\n", 215 | i, det->family->nbits, det->family->h, det->id, 216 | det->hamming, det->decision_margin); 217 | 218 | // ignore untracked tags 219 | if(!tag_frames.empty() && !tag_frames.count(det->id)) { continue; } 220 | 221 | // reject detections with more corrected bits than allowed 222 | if(det->hamming > max_hamming) { continue; } 223 | 224 | // detection 225 | apriltag_msgs::msg::AprilTagDetection msg_detection; 226 | msg_detection.family = std::string(det->family->name); 227 | msg_detection.id = det->id; 228 | msg_detection.hamming = det->hamming; 229 | msg_detection.decision_margin = det->decision_margin; 230 | msg_detection.centre.x = det->c[0]; 231 | msg_detection.centre.y = det->c[1]; 232 | std::memcpy(msg_detection.corners.data(), det->p, sizeof(double) * 8); 233 | std::memcpy(msg_detection.homography.data(), det->H->data, sizeof(double) * 9); 234 | msg_detections.detections.push_back(msg_detection); 235 | 236 | // 3D orientation and position 237 | if(estimate_pose != nullptr && calibrated) { 238 | geometry_msgs::msg::TransformStamped tf; 239 | tf.header = msg_img->header; 240 | // set child frame name by generic tag name or configured tag name 241 | tf.child_frame_id = tag_frames.count(det->id) ? tag_frames.at(det->id) : std::string(det->family->name) + ":" + std::to_string(det->id); 242 | const double size = tag_sizes.count(det->id) ? tag_sizes.at(det->id) : tag_edge_size; 243 | tf.transform = estimate_pose(det, intrinsics, size); 244 | tfs.push_back(tf); 245 | } 246 | } 247 | 248 | pub_detections->publish(msg_detections); 249 | 250 | if(estimate_pose != nullptr) 251 | tf_broadcaster.sendTransform(tfs); 252 | 253 | apriltag_detections_destroy(detections); 254 | } 255 | 256 | rcl_interfaces::msg::SetParametersResult 257 | AprilTagNode::onParameter(const std::vector& parameters) 258 | { 259 | rcl_interfaces::msg::SetParametersResult result; 260 | 261 | mutex.lock(); 262 | 263 | for(const rclcpp::Parameter& parameter : parameters) { 264 | RCLCPP_DEBUG_STREAM(get_logger(), "setting: " << parameter); 265 | 266 | IF("detector.threads", td->nthreads) 267 | IF("detector.decimate", td->quad_decimate) 268 | IF("detector.blur", td->quad_sigma) 269 | IF("detector.refine", td->refine_edges) 270 | IF("detector.sharpening", td->decode_sharpening) 271 | IF("detector.debug", td->debug) 272 | IF("max_hamming", max_hamming) 273 | IF("profile", profile) 274 | } 275 | 276 | mutex.unlock(); 277 | 278 | result.successful = true; 279 | 280 | return result; 281 | } 282 | -------------------------------------------------------------------------------- /src/conversion.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | template<> 10 | void tf2::convert(const Eigen::Quaterniond& eigen_quat, geometry_msgs::msg::Quaternion& msg_quat) 11 | { 12 | msg_quat.w = eigen_quat.w(); 13 | msg_quat.x = eigen_quat.x(); 14 | msg_quat.y = eigen_quat.y(); 15 | msg_quat.z = eigen_quat.z(); 16 | } 17 | 18 | template<> 19 | void tf2::convert(const matd_t& mat, geometry_msgs::msg::Vector3& msg_vec) 20 | { 21 | assert((mat.nrows == 3 && mat.ncols == 1) || (mat.nrows == 1 && mat.ncols == 3)); 22 | 23 | msg_vec.x = mat.data[0]; 24 | msg_vec.y = mat.data[1]; 25 | msg_vec.z = mat.data[2]; 26 | } 27 | 28 | template<> 29 | void tf2::convert(const cv::Mat_& vec, geometry_msgs::msg::Vector3& msg_vec) 30 | { 31 | assert((vec.rows == 3 && vec.cols == 1) || (vec.rows == 1 && vec.cols == 3)); 32 | 33 | msg_vec.x = vec.at(0); 34 | msg_vec.y = vec.at(1); 35 | msg_vec.z = vec.at(2); 36 | } 37 | 38 | template<> 39 | geometry_msgs::msg::Transform 40 | tf2::toMsg(const apriltag_pose_t& pose) 41 | { 42 | const Eigen::Quaterniond q(Eigen::Map>(pose.R->data)); 43 | 44 | geometry_msgs::msg::Transform t; 45 | tf2::convert(*pose.t, t.translation); 46 | tf2::convert(q, t.rotation); 47 | return t; 48 | } 49 | 50 | template<> 51 | geometry_msgs::msg::Transform 52 | tf2::toMsg(const std::pair, cv::Mat_>& pose) 53 | { 54 | assert((pose.first.rows == 3 && pose.first.cols == 1) || (pose.first.rows == 1 && pose.first.cols == 3)); 55 | assert((pose.second.rows == 3 && pose.second.cols == 1) || (pose.second.rows == 1 && pose.second.cols == 3)); 56 | 57 | // convert compact rotation vector to angle-axis to quaternion 58 | const Eigen::Map rvec(reinterpret_cast(pose.second.data)); 59 | const Eigen::Quaterniond q({rvec.norm(), rvec.normalized()}); 60 | 61 | geometry_msgs::msg::Transform t; 62 | tf2::convert(pose.first, t.translation); 63 | tf2::convert(q, t.rotation); 64 | return t; 65 | } 66 | -------------------------------------------------------------------------------- /src/pose_estimation.cpp: -------------------------------------------------------------------------------- 1 | #include "pose_estimation.hpp" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | geometry_msgs::msg::Transform 10 | homography(apriltag_detection_t* const detection, const std::array& intr, double tagsize) 11 | { 12 | apriltag_detection_info_t info = {detection, tagsize, intr[0], intr[1], intr[2], intr[3]}; 13 | 14 | apriltag_pose_t pose; 15 | estimate_pose_for_tag_homography(&info, &pose); 16 | 17 | // rotate frame such that z points in the opposite direction towards the camera 18 | for(int i = 0; i < 3; i++) { 19 | // swap x and y axes 20 | std::swap(MATD_EL(pose.R, 0, i), MATD_EL(pose.R, 1, i)); 21 | // invert z axis 22 | MATD_EL(pose.R, 2, i) *= -1; 23 | } 24 | 25 | return tf2::toMsg(const_cast(pose)); 26 | } 27 | 28 | geometry_msgs::msg::Transform 29 | pnp(apriltag_detection_t* const detection, const std::array& intr, double tagsize) 30 | { 31 | const std::vector objectPoints{ 32 | {-tagsize / 2, -tagsize / 2, 0}, 33 | {+tagsize / 2, -tagsize / 2, 0}, 34 | {+tagsize / 2, +tagsize / 2, 0}, 35 | {-tagsize / 2, +tagsize / 2, 0}, 36 | }; 37 | 38 | const std::vector imagePoints{ 39 | {detection->p[0][0], detection->p[0][1]}, 40 | {detection->p[1][0], detection->p[1][1]}, 41 | {detection->p[2][0], detection->p[2][1]}, 42 | {detection->p[3][0], detection->p[3][1]}, 43 | }; 44 | 45 | cv::Matx33d cameraMatrix; 46 | cameraMatrix(0, 0) = intr[0];// fx 47 | cameraMatrix(1, 1) = intr[1];// fy 48 | cameraMatrix(0, 2) = intr[2];// cx 49 | cameraMatrix(1, 2) = intr[3];// cy 50 | 51 | cv::Mat rvec, tvec; 52 | cv::solvePnP(objectPoints, imagePoints, cameraMatrix, {}, rvec, tvec); 53 | 54 | return tf2::toMsg, cv::Mat_>, geometry_msgs::msg::Transform>(std::make_pair(tvec, rvec)); 55 | } 56 | 57 | const std::unordered_map pose_estimation_methods{ 58 | {"homography", homography}, 59 | {"pnp", pnp}, 60 | }; 61 | -------------------------------------------------------------------------------- /src/pose_estimation.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | typedef std::function&, const double&)> pose_estimation_f; 10 | 11 | extern const std::unordered_map pose_estimation_methods; 12 | -------------------------------------------------------------------------------- /src/tag_functions.cpp: -------------------------------------------------------------------------------- 1 | #include "tag_functions.hpp" 2 | 3 | // default tag families 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | // create and delete functions for default tags 14 | #define TAG_FUN(name) {#name, {tag##name##_create, tag##name##_destroy}}, 15 | 16 | // function pointer for tag family creation / destruction 17 | const std::unordered_map> tag_fun{ 18 | // clang-format off 19 | TAG_FUN(36h11) 20 | TAG_FUN(25h9) 21 | TAG_FUN(16h5) 22 | TAG_FUN(Circle21h7) 23 | TAG_FUN(Circle49h12) 24 | TAG_FUN(Custom48h12) 25 | TAG_FUN(Standard41h12) 26 | TAG_FUN(Standard52h13) 27 | // clang-format on 28 | }; 29 | -------------------------------------------------------------------------------- /src/tag_functions.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | extern const std::unordered_map> tag_fun; 9 | --------------------------------------------------------------------------------