From 12190d8217a54c635b875544e47d1222597eefa0 Mon Sep 17 00:00:00 2001 From: Jack Zender Date: Thu, 20 Apr 2023 15:50:13 +0000 Subject: [PATCH 1/8] tag bundle calibrator, need to test on bench --- .gitignore | 1 + apriltag_ros/CMakeLists.txt | 12 ++ .../apriltag_ros/tag_bundle_calibration.h | 62 ++++++ apriltag_ros/package.xml | 50 ++--- ...riltag_ros_tag_bundle_calibration_node.cpp | 26 +++ apriltag_ros/src/tag_bundle_calibration.cpp | 194 ++++++++++++++++++ 6 files changed, 314 insertions(+), 31 deletions(-) create mode 100644 apriltag_ros/include/apriltag_ros/tag_bundle_calibration.h create mode 100644 apriltag_ros/src/apriltag_ros_tag_bundle_calibration_node.cpp create mode 100644 apriltag_ros/src/tag_bundle_calibration.cpp diff --git a/.gitignore b/.gitignore index 8c303ba3..c838e0c3 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,4 @@ **/*.bag .vscode/ build/ +.clang-format diff --git a/apriltag_ros/CMakeLists.txt b/apriltag_ros/CMakeLists.txt index a933668a..00b38fb0 100644 --- a/apriltag_ros/CMakeLists.txt +++ b/apriltag_ros/CMakeLists.txt @@ -16,6 +16,8 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs std_msgs tf + tf2 + tf2_geometry_msgs ) find_package(Eigen3 REQUIRED) @@ -85,6 +87,8 @@ catkin_package( sensor_msgs std_msgs tf + tf2 + tf2_geometry_msgs DEPENDS apriltag OpenCV @@ -108,6 +112,9 @@ add_library(${PROJECT_NAME}_common src/common_functions.cpp) add_dependencies(${PROJECT_NAME}_common ${PROJECT_NAME}_generate_messages_cpp) target_link_libraries(${PROJECT_NAME}_common ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} apriltag::apriltag) +add_library(${PROJECT_NAME}_tag_bundle_calibration src/tag_bundle_calibration.cpp) +target_link_libraries(${PROJECT_NAME}_tag_bundle_calibration ${PROJECT_NAME}_common ${catkin_LIBRARIES}) + add_library(${PROJECT_NAME}_continuous_detector src/continuous_detector.cpp) target_link_libraries(${PROJECT_NAME}_continuous_detector ${PROJECT_NAME}_common ${catkin_LIBRARIES}) @@ -126,6 +133,10 @@ add_executable(${PROJECT_NAME}_single_image_client_node src/${PROJECT_NAME}_sing add_dependencies(${PROJECT_NAME}_single_image_client_node ${PROJECT_NAME}_generate_messages_cpp) target_link_libraries(${PROJECT_NAME}_single_image_client_node ${PROJECT_NAME}_common ${catkin_LIBRARIES}) +add_executable(${PROJECT_NAME}_tag_bundle_calibration_node src/${PROJECT_NAME}_tag_bundle_calibration_node.cpp) +add_dependencies(${PROJECT_NAME}_tag_bundle_calibration_node ${PROJECT_NAME}_generate_messages_cpp) +target_link_libraries(${PROJECT_NAME}_tag_bundle_calibration_node ${PROJECT_NAME}_tag_bundle_calibration ${catkin_LIBRARIES}) + ############# ## Install ## @@ -145,6 +156,7 @@ install(FILES nodelet_plugins.xml install(TARGETS ${PROJECT_NAME}_common + ${PROJECT_NAME}_tag_bundle_calibration ${PROJECT_NAME}_continuous_detector ${PROJECT_NAME}_continuous_node ${PROJECT_NAME}_single_image_client_node diff --git a/apriltag_ros/include/apriltag_ros/tag_bundle_calibration.h b/apriltag_ros/include/apriltag_ros/tag_bundle_calibration.h new file mode 100644 index 00000000..06cfd862 --- /dev/null +++ b/apriltag_ros/include/apriltag_ros/tag_bundle_calibration.h @@ -0,0 +1,62 @@ +#ifndef APRILTAG_ROS_TAG_BUNDLE_CALIBRATION_H +#define APRILTAG_ROS_TAG_BUNDLE_CALIBRATION_H + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace apriltag_ros +{ +void generateTagBundleConfig( + std::ostream& os, + const std::unordered_map>& tag_buffer_map, + const std::string tag_bundle_name, + const int master_tag_id); + +namespace detail +{ + +geometry_msgs::Pose averagePoses(const std::vector& poses); +void writeToYaml(const std::unordered_map& tag_poses_in_master_frame, + const std::unordered_map& tag_size_map, + const std::string& tag_bundle_name, + std::ostream& os); +} // namespace detail + +class TagBundleCalibrationNode +{ +public: + TagBundleCalibrationNode(ros::NodeHandle& nh, + int max_detections, + const std::string& config_file_path, + const std::string& tag_bundle_name, + int master_tag_id); + + void tagDetectionCallback(const apriltag_ros::AprilTagDetectionArray::ConstPtr& msg); + +private: + void performCalibrationAndWriteToFile(); + + ros::Subscriber tag_detection_sub_; + int max_detections_; + std::string config_file_path_; + std::string tag_bundle_name_; + int master_tag_id_; + int received_detections_; + std::unordered_map> tag_buffer_map_; +}; + +} // namespace apriltag_ros + +#endif // APRILTAG_ROS_TAG_BUNDLE_CALIBRATION_H diff --git a/apriltag_ros/package.xml b/apriltag_ros/package.xml index da33a776..471f4bb0 100644 --- a/apriltag_ros/package.xml +++ b/apriltag_ros/package.xml @@ -1,5 +1,5 @@ - + apriltag_ros 3.2.1 @@ -26,37 +26,25 @@ catkin - apriltag - geometry_msgs - image_transport - image_geometry - roscpp - sensor_msgs - message_generation - std_msgs - cv_bridge - tf - nodelet - pluginlib - eigen - libopencv-dev - cmake_modules + apriltag + geometry_msgs + image_transport + image_geometry + roscpp + sensor_msgs + message_generation + message_runtime + std_msgs + cv_bridge + tf + nodelet + pluginlib + eigen + libopencv-dev + cmake_modules + tf2 + tf2_geometry_msgs - tf - apriltag - geometry_msgs - image_transport - image_geometry - roscpp - sensor_msgs - message_runtime - std_msgs - cv_bridge - nodelet - pluginlib - eigen - libopencv-dev - diff --git a/apriltag_ros/src/apriltag_ros_tag_bundle_calibration_node.cpp b/apriltag_ros/src/apriltag_ros_tag_bundle_calibration_node.cpp new file mode 100644 index 00000000..28f4aab5 --- /dev/null +++ b/apriltag_ros/src/apriltag_ros_tag_bundle_calibration_node.cpp @@ -0,0 +1,26 @@ +#include "apriltag_ros/tag_bundle_calibration.h" +#include + +int main(int argc, char** argv) +{ + // Initialize the node and create a NodeHandle + ros::init(argc, argv, "tag_bundle_calibration_node"); + ros::NodeHandle nh; + + // Read the parameters + int max_detections; + std::string config_file_path; + std::string tag_bundle_name; + int master_tag_id; + nh.param("max_detections", max_detections, 500); + nh.param("config_file_path", config_file_path, std::string("/tmp/tag_bundle_config.yaml")); + nh.param("tag_bundle_name", tag_bundle_name, std::string("tag_bundle")); + nh.param("master_tag_id", master_tag_id, 0); + + // Create the node + apriltag_ros::TagBundleCalibrationNode node( + nh, max_detections, config_file_path, tag_bundle_name, master_tag_id); + + ros::spin(); + return 0; +} \ No newline at end of file diff --git a/apriltag_ros/src/tag_bundle_calibration.cpp b/apriltag_ros/src/tag_bundle_calibration.cpp new file mode 100644 index 00000000..79f07c7b --- /dev/null +++ b/apriltag_ros/src/tag_bundle_calibration.cpp @@ -0,0 +1,194 @@ +#include + +namespace apriltag_ros +{ +void generateTagBundleConfig( + std::ostream& os, + const std::unordered_map>& tag_buffer_map, + const std::string tag_bundle_name, + const int master_tag_id) +{ + if (tag_buffer_map.find(master_tag_id) == tag_buffer_map.end()) + { + throw std::runtime_error("Master tag ID has not been observed"); + } + + // Calculating the average pose for each tag + std::unordered_map average_pose_map; + std::unordered_map tag_size_map; + for (const auto& [tag_id, detections] : tag_buffer_map) + { + // Extracting the poses from the detections + std::vector poses; + poses.reserve(detections.size()); + for (const auto& detection : detections) + { + // Ignore tag bundle detections in the tag bundle calibration routine + if (detection.id.size() > 1) + continue; + poses.push_back(detection.pose.pose.pose); + } + + // Averaging the poses + average_pose_map[tag_id] = detail::averagePoses(poses); + tag_size_map[tag_id] = detections[0].size[0]; + } + + // Get the master tag's pose + const geometry_msgs::Pose& master_pose = average_pose_map[master_tag_id]; + + // Convert the master pose to a tf2::Transform + tf2::Transform master_tf; + tf2::fromMsg(master_pose, master_tf); + + // Compute the tag poses in the frame of the master tag + std::unordered_map tag_poses_in_master_frame; + tag_poses_in_master_frame[master_tag_id] = geometry_msgs::Pose(); + for (const auto& [tag_id, pose] : average_pose_map) + { + if (tag_id == master_tag_id) + continue; + + // Convert the tag pose to a tf2::Transform + tf2::Transform tag_tf; + tf2::fromMsg(pose, tag_tf); + + // Calculate the transform from the master tag to the current tag + tf2::Transform transform_in_master_frame = master_tf.inverse() * tag_tf; + + // Convert the transform back to a geometry_msgs::Pose and store it + tag_poses_in_master_frame[tag_id] = geometry_msgs::Pose(); + tf2::toMsg(transform_in_master_frame, tag_poses_in_master_frame[tag_id]); + } + + // Pass the tag map and the output stream to the writeToYaml function + detail::writeToYaml(tag_poses_in_master_frame, tag_size_map, tag_bundle_name, os); +} + +namespace detail +{ +geometry_msgs::Pose averagePoses(const std::vector& poses) +{ + geometry_msgs::Pose ret; + + if (poses.empty()) + return ret; + + // Averaging positions + double sz = static_cast(poses.size()); + geometry_msgs::Point init_position; + geometry_msgs::Point avg_position = + std::accumulate(poses.begin(), + poses.end(), + init_position, + [sz](geometry_msgs::Point& accum, const geometry_msgs::Pose& curr) + { + accum.x += curr.position.x / sz; + accum.y += curr.position.y / sz; + accum.z += curr.position.z / sz; + return accum; + }); + + ret.position = avg_position; + + // Averaging orientations + // https://math.stackexchange.com/questions/61146/averaging-quaternions/3435296#3435296 + tf2::Quaternion init_quat{0.0, 0.0, 0.0, 0.0}; + tf2::Quaternion first_quat, curr_quat; + tf2::fromMsg(poses[0].orientation, first_quat); + + auto avg_quat_approx = std::accumulate( + poses.begin(), + poses.end(), + init_quat, + [&first_quat, &curr_quat](tf2::Quaternion& accum, const geometry_msgs::Pose& curr) + { + tf2::fromMsg(curr.orientation, curr_quat); + double weight = curr_quat.dot(first_quat) > 0.0 ? 1.0 : -1.0; + return accum + tf2::Quaternion(weight * curr_quat.x(), + weight * curr_quat.y(), + weight * curr_quat.z(), + weight * curr_quat.w()); + }); + + ret.orientation = tf2::toMsg(avg_quat_approx.normalize()); + + return ret; +} + +void writeToYaml(const std::unordered_map& tag_poses_in_master_frame, + const std::unordered_map& tag_size_map, + const std::string& tag_bundle_name, + std::ostream& os) +{ + os << "standalone_tags:\n"; + os << " [\n"; + os << " ]\n"; + os << "tag_bundles:\n"; + os << " [\n"; + os << " {\n"; + os << " name: '" << tag_bundle_name << "',\n"; + os << " layout:\n"; + os << " [\n"; + + for (const auto& [tag_id, pose] : tag_poses_in_master_frame) + { + double size = tag_size_map.at(tag_id); + os << " {id: " << tag_id << ", size: " << size << ", x: " << pose.position.x + << ", y: " << pose.position.y << ", z: " << pose.position.z << ", qw: " << pose.orientation.w + << ", qx: " << pose.orientation.x << ", qy: " << pose.orientation.y + << ", qz: " << pose.orientation.z << "},\n"; + } + + os << " ]\n"; + os << " }\n"; + os << " ]\n"; +} + +} // namespace detail + +// TagBundleCalibrationNode class +TagBundleCalibrationNode::TagBundleCalibrationNode(ros::NodeHandle& nh, + int max_detections, + const std::string& config_file_path, + const std::string& tag_bundle_name, + int master_tag_id) : + max_detections_(max_detections), + config_file_path_(config_file_path), + tag_bundle_name_(tag_bundle_name), + master_tag_id_(master_tag_id), + received_detections_(0) +{ + tag_detection_sub_ = + nh.subscribe("tag_detection", 1, &TagBundleCalibrationNode::tagDetectionCallback, this); +} + +void TagBundleCalibrationNode::tagDetectionCallback( + const apriltag_ros::AprilTagDetectionArray::ConstPtr& msg) +{ + for (const auto& detection : msg->detections) + { + // Only standalone tags + if (detection.id.size() == 1) + { + if (tag_buffer_map_.find(detection.id[0]) == tag_buffer_map_.end()) + tag_buffer_map_[detection.id[0]] = {}; + tag_buffer_map_[detection.id[0]].push_back(detection); + } + } + + received_detections_++; + + if (received_detections_ >= max_detections_) + { + performCalibrationAndWriteToFile(); + ros::shutdown(); + } +} + +void TagBundleCalibrationNode::performCalibrationAndWriteToFile() +{ + std::ofstream ofs(config_file_path_); + apriltag_ros::generateTagBundleConfig(ofs, tag_buffer_map_, tag_bundle_name_, master_tag_id_); +} +} // namespace apriltag_ros From 8e3d1c5ff83814689d8be697d31292346e854936 Mon Sep 17 00:00:00 2001 From: Jack Zender Date: Thu, 20 Apr 2023 16:14:41 +0000 Subject: [PATCH 2/8] tag bundle calib launch --- .../include/apriltag_ros/tag_bundle_calibration.h | 3 +-- apriltag_ros/launch/tag_bundle_calibration.launch | 15 +++++++++++++++ .../apriltag_ros_tag_bundle_calibration_node.cpp | 2 +- apriltag_ros/src/tag_bundle_calibration.cpp | 6 +++--- 4 files changed, 20 insertions(+), 6 deletions(-) create mode 100644 apriltag_ros/launch/tag_bundle_calibration.launch diff --git a/apriltag_ros/include/apriltag_ros/tag_bundle_calibration.h b/apriltag_ros/include/apriltag_ros/tag_bundle_calibration.h index 06cfd862..436c91c7 100644 --- a/apriltag_ros/include/apriltag_ros/tag_bundle_calibration.h +++ b/apriltag_ros/include/apriltag_ros/tag_bundle_calibration.h @@ -37,8 +37,7 @@ void writeToYaml(const std::unordered_map& tag_poses_i class TagBundleCalibrationNode { public: - TagBundleCalibrationNode(ros::NodeHandle& nh, - int max_detections, + TagBundleCalibrationNode(int max_detections, const std::string& config_file_path, const std::string& tag_bundle_name, int master_tag_id); diff --git a/apriltag_ros/launch/tag_bundle_calibration.launch b/apriltag_ros/launch/tag_bundle_calibration.launch new file mode 100644 index 00000000..c2c31515 --- /dev/null +++ b/apriltag_ros/launch/tag_bundle_calibration.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/apriltag_ros/src/apriltag_ros_tag_bundle_calibration_node.cpp b/apriltag_ros/src/apriltag_ros_tag_bundle_calibration_node.cpp index 28f4aab5..22363bbe 100644 --- a/apriltag_ros/src/apriltag_ros_tag_bundle_calibration_node.cpp +++ b/apriltag_ros/src/apriltag_ros_tag_bundle_calibration_node.cpp @@ -19,7 +19,7 @@ int main(int argc, char** argv) // Create the node apriltag_ros::TagBundleCalibrationNode node( - nh, max_detections, config_file_path, tag_bundle_name, master_tag_id); + max_detections, config_file_path, tag_bundle_name, master_tag_id); ros::spin(); return 0; diff --git a/apriltag_ros/src/tag_bundle_calibration.cpp b/apriltag_ros/src/tag_bundle_calibration.cpp index 79f07c7b..f33c7872 100644 --- a/apriltag_ros/src/tag_bundle_calibration.cpp +++ b/apriltag_ros/src/tag_bundle_calibration.cpp @@ -148,8 +148,7 @@ void writeToYaml(const std::unordered_map& tag_poses_i } // namespace detail // TagBundleCalibrationNode class -TagBundleCalibrationNode::TagBundleCalibrationNode(ros::NodeHandle& nh, - int max_detections, +TagBundleCalibrationNode::TagBundleCalibrationNode(int max_detections, const std::string& config_file_path, const std::string& tag_bundle_name, int master_tag_id) : @@ -159,8 +158,9 @@ TagBundleCalibrationNode::TagBundleCalibrationNode(ros::NodeHandle& nh, master_tag_id_(master_tag_id), received_detections_(0) { + ros::NodeHandle pnh{"~"}; tag_detection_sub_ = - nh.subscribe("tag_detection", 1, &TagBundleCalibrationNode::tagDetectionCallback, this); + pnh.subscribe("tag_detections", 1, &TagBundleCalibrationNode::tagDetectionCallback, this); } void TagBundleCalibrationNode::tagDetectionCallback( From 912ab682b87035eeaa6b3cba6c93085de423ce8e Mon Sep 17 00:00:00 2001 From: Jack Zender Date: Fri, 21 Apr 2023 13:57:35 +0000 Subject: [PATCH 3/8] fix logic error in previously implementation, we know calculate tf to master id on each tag observation and average the tfs over time --- .../apriltag_ros/tag_bundle_calibration.h | 13 +- ...riltag_ros_tag_bundle_calibration_node.cpp | 2 +- apriltag_ros/src/tag_bundle_calibration.cpp | 164 ++++++++++-------- 3 files changed, 91 insertions(+), 88 deletions(-) diff --git a/apriltag_ros/include/apriltag_ros/tag_bundle_calibration.h b/apriltag_ros/include/apriltag_ros/tag_bundle_calibration.h index 436c91c7..33830bc7 100644 --- a/apriltag_ros/include/apriltag_ros/tag_bundle_calibration.h +++ b/apriltag_ros/include/apriltag_ros/tag_bundle_calibration.h @@ -18,12 +18,6 @@ namespace apriltag_ros { -void generateTagBundleConfig( - std::ostream& os, - const std::unordered_map>& tag_buffer_map, - const std::string tag_bundle_name, - const int master_tag_id); - namespace detail { @@ -42,10 +36,8 @@ class TagBundleCalibrationNode const std::string& tag_bundle_name, int master_tag_id); - void tagDetectionCallback(const apriltag_ros::AprilTagDetectionArray::ConstPtr& msg); - private: - void performCalibrationAndWriteToFile(); + void tagDetectionCallback(const apriltag_ros::AprilTagDetectionArray::ConstPtr& msg); ros::Subscriber tag_detection_sub_; int max_detections_; @@ -53,7 +45,8 @@ class TagBundleCalibrationNode std::string tag_bundle_name_; int master_tag_id_; int received_detections_; - std::unordered_map> tag_buffer_map_; + std::unordered_map tags_in_master_frame_; + std::unordered_map tag_size_map_; }; } // namespace apriltag_ros diff --git a/apriltag_ros/src/apriltag_ros_tag_bundle_calibration_node.cpp b/apriltag_ros/src/apriltag_ros_tag_bundle_calibration_node.cpp index 22363bbe..97986049 100644 --- a/apriltag_ros/src/apriltag_ros_tag_bundle_calibration_node.cpp +++ b/apriltag_ros/src/apriltag_ros_tag_bundle_calibration_node.cpp @@ -5,7 +5,7 @@ int main(int argc, char** argv) { // Initialize the node and create a NodeHandle ros::init(argc, argv, "tag_bundle_calibration_node"); - ros::NodeHandle nh; + ros::NodeHandle nh{"~"}; // Read the parameters int max_detections; diff --git a/apriltag_ros/src/tag_bundle_calibration.cpp b/apriltag_ros/src/tag_bundle_calibration.cpp index f33c7872..98518897 100644 --- a/apriltag_ros/src/tag_bundle_calibration.cpp +++ b/apriltag_ros/src/tag_bundle_calibration.cpp @@ -2,68 +2,6 @@ namespace apriltag_ros { -void generateTagBundleConfig( - std::ostream& os, - const std::unordered_map>& tag_buffer_map, - const std::string tag_bundle_name, - const int master_tag_id) -{ - if (tag_buffer_map.find(master_tag_id) == tag_buffer_map.end()) - { - throw std::runtime_error("Master tag ID has not been observed"); - } - - // Calculating the average pose for each tag - std::unordered_map average_pose_map; - std::unordered_map tag_size_map; - for (const auto& [tag_id, detections] : tag_buffer_map) - { - // Extracting the poses from the detections - std::vector poses; - poses.reserve(detections.size()); - for (const auto& detection : detections) - { - // Ignore tag bundle detections in the tag bundle calibration routine - if (detection.id.size() > 1) - continue; - poses.push_back(detection.pose.pose.pose); - } - - // Averaging the poses - average_pose_map[tag_id] = detail::averagePoses(poses); - tag_size_map[tag_id] = detections[0].size[0]; - } - - // Get the master tag's pose - const geometry_msgs::Pose& master_pose = average_pose_map[master_tag_id]; - - // Convert the master pose to a tf2::Transform - tf2::Transform master_tf; - tf2::fromMsg(master_pose, master_tf); - - // Compute the tag poses in the frame of the master tag - std::unordered_map tag_poses_in_master_frame; - tag_poses_in_master_frame[master_tag_id] = geometry_msgs::Pose(); - for (const auto& [tag_id, pose] : average_pose_map) - { - if (tag_id == master_tag_id) - continue; - - // Convert the tag pose to a tf2::Transform - tf2::Transform tag_tf; - tf2::fromMsg(pose, tag_tf); - - // Calculate the transform from the master tag to the current tag - tf2::Transform transform_in_master_frame = master_tf.inverse() * tag_tf; - - // Convert the transform back to a geometry_msgs::Pose and store it - tag_poses_in_master_frame[tag_id] = geometry_msgs::Pose(); - tf2::toMsg(transform_in_master_frame, tag_poses_in_master_frame[tag_id]); - } - - // Pass the tag map and the output stream to the writeToYaml function - detail::writeToYaml(tag_poses_in_master_frame, tag_size_map, tag_bundle_name, os); -} namespace detail { @@ -121,6 +59,15 @@ void writeToYaml(const std::unordered_map& tag_poses_i const std::string& tag_bundle_name, std::ostream& os) { + // Create a vector of constant references to the elements in tag_poses_in_master_frame + std::vector>> sorted_tags( + tag_poses_in_master_frame.begin(), tag_poses_in_master_frame.end()); + + // Sort the vector by ascending tag ID + std::sort(sorted_tags.begin(), + sorted_tags.end(), + [](const auto& a, const auto& b) { return a.get().first < b.get().first; }); + os << "standalone_tags:\n"; os << " [\n"; os << " ]\n"; @@ -131,8 +78,11 @@ void writeToYaml(const std::unordered_map& tag_poses_i os << " layout:\n"; os << " [\n"; - for (const auto& [tag_id, pose] : tag_poses_in_master_frame) + for (const auto& tag_entry_ref : sorted_tags) { + const auto& tag_entry = tag_entry_ref.get(); + int tag_id = tag_entry.first; + const geometry_msgs::Pose& pose = tag_entry.second; double size = tag_size_map.at(tag_id); os << " {id: " << tag_id << ", size: " << size << ", x: " << pose.position.x << ", y: " << pose.position.y << ", z: " << pose.position.z << ", qw: " << pose.orientation.w @@ -159,21 +109,79 @@ TagBundleCalibrationNode::TagBundleCalibrationNode(int max_detections, received_detections_(0) { ros::NodeHandle pnh{"~"}; - tag_detection_sub_ = - pnh.subscribe("tag_detections", 1, &TagBundleCalibrationNode::tagDetectionCallback, this); + size_t q_size = max_detections_ + 1; // Should allow faster than real-time stuff w rosbags + tag_detection_sub_ = pnh.subscribe( + "tag_detections", q_size, &TagBundleCalibrationNode::tagDetectionCallback, this); + + ROS_INFO_STREAM("Initialized tag bundle calibration node with the following parameters:\n" + << " max_detections: " << max_detections_ << "\n" + << " config_file_path: " << config_file_path_ << "\n" + << " tag_bundle_name: " << tag_bundle_name_ << "\n" + << " master_tag_id: " << master_tag_id_ << "\n"); } void TagBundleCalibrationNode::tagDetectionCallback( const apriltag_ros::AprilTagDetectionArray::ConstPtr& msg) { - for (const auto& detection : msg->detections) + std::unordered_map curr_tag_poses_in_cam_frame; + + for (const auto& tag_detection : msg->detections) + { + int tag_id = tag_detection.id[0]; + double tag_size = tag_detection.size[0]; + geometry_msgs::Pose tag_pose_in_master_frame = tag_detection.pose.pose.pose; + + curr_tag_poses_in_cam_frame[tag_id] = tag_pose_in_master_frame; + // Build up the tag size map + if (tag_size_map_.find(tag_id) == tag_size_map_.end()) + tag_size_map_[tag_id] = tag_size; + } + + // If the master tag is not detected, skip this detection + if (curr_tag_poses_in_cam_frame.find(master_tag_id_) == curr_tag_poses_in_cam_frame.end()) + return; + + // Transform all tag poses to the master tag frame + geometry_msgs::Pose master_tag_pose = curr_tag_poses_in_cam_frame[master_tag_id_]; + tf2::Transform master_tag_tf; + tf2::fromMsg(master_tag_pose, master_tag_tf); + + // init the master tag pose if it is not already in the map + if (tags_in_master_frame_.find(master_tag_id_) == tags_in_master_frame_.end()) + tags_in_master_frame_[master_tag_id_] = geometry_msgs::Pose{}; + + for (auto& tag_pose_kv : curr_tag_poses_in_cam_frame) { - // Only standalone tags - if (detection.id.size() == 1) + if (tag_pose_kv.first == master_tag_id_) + continue; + + tf2::Transform tag_tf; + tf2::fromMsg(tag_pose_kv.second, tag_tf); + + tf2::Transform relative_tf = master_tag_tf.inverse() * tag_tf; + geometry_msgs::Pose relative_pose; + tf2::toMsg(relative_tf, relative_pose); + + int tag_id = tag_pose_kv.first; + // If this tag is not in tags_in_master_frame_, add it. + if (tags_in_master_frame_.find(tag_id) == tags_in_master_frame_.end()) + { + tags_in_master_frame_[tag_id] = relative_pose; + } + else { - if (tag_buffer_map_.find(detection.id[0]) == tag_buffer_map_.end()) - tag_buffer_map_[detection.id[0]] = {}; - tag_buffer_map_[detection.id[0]].push_back(detection); + // Average the current pose with the previously stored pose. + geometry_msgs::Pose& prev_pose = tags_in_master_frame_[tag_id]; + + prev_pose.position.x = (prev_pose.position.x + relative_pose.position.x) / 2; + prev_pose.position.y = (prev_pose.position.y + relative_pose.position.y) / 2; + prev_pose.position.z = (prev_pose.position.z + relative_pose.position.z) / 2; + + tf2::Quaternion prev_quat, relative_quat; + tf2::fromMsg(prev_pose.orientation, prev_quat); + tf2::fromMsg(relative_pose.orientation, relative_quat); + prev_quat = prev_quat.slerp(relative_quat, 0.5); + prev_pose.orientation = tf2::toMsg(prev_quat); } } @@ -181,14 +189,16 @@ void TagBundleCalibrationNode::tagDetectionCallback( if (received_detections_ >= max_detections_) { - performCalibrationAndWriteToFile(); + std::ofstream ofs{config_file_path_}; + if (!ofs.is_open()) + { + ROS_ERROR_STREAM("Failed to open file: " << config_file_path_); + ros::shutdown(); + return; + } + detail::writeToYaml(tags_in_master_frame_, tag_size_map_, tag_bundle_name_, ofs); ros::shutdown(); } } -void TagBundleCalibrationNode::performCalibrationAndWriteToFile() -{ - std::ofstream ofs(config_file_path_); - apriltag_ros::generateTagBundleConfig(ofs, tag_buffer_map_, tag_bundle_name_, master_tag_id_); -} } // namespace apriltag_ros From e877d609dc14cbbc2baf16db1a59a20d392b069f Mon Sep 17 00:00:00 2001 From: Jack Zender Date: Fri, 21 Apr 2023 14:00:18 +0000 Subject: [PATCH 4/8] some refactorings --- .../apriltag_ros/tag_bundle_calibration.h | 15 +- apriltag_ros/src/tag_bundle_calibration.cpp | 140 ++++++------------ 2 files changed, 48 insertions(+), 107 deletions(-) diff --git a/apriltag_ros/include/apriltag_ros/tag_bundle_calibration.h b/apriltag_ros/include/apriltag_ros/tag_bundle_calibration.h index 33830bc7..72a55330 100644 --- a/apriltag_ros/include/apriltag_ros/tag_bundle_calibration.h +++ b/apriltag_ros/include/apriltag_ros/tag_bundle_calibration.h @@ -18,16 +18,6 @@ namespace apriltag_ros { -namespace detail -{ - -geometry_msgs::Pose averagePoses(const std::vector& poses); -void writeToYaml(const std::unordered_map& tag_poses_in_master_frame, - const std::unordered_map& tag_size_map, - const std::string& tag_bundle_name, - std::ostream& os); -} // namespace detail - class TagBundleCalibrationNode { public: @@ -39,6 +29,11 @@ class TagBundleCalibrationNode private: void tagDetectionCallback(const apriltag_ros::AprilTagDetectionArray::ConstPtr& msg); + void writeToYaml(const std::unordered_map& tag_poses_in_master_frame, + const std::unordered_map& tag_size_map, + const std::string& tag_bundle_name, + std::ostream& os); + ros::Subscriber tag_detection_sub_; int max_detections_; std::string config_file_path_; diff --git a/apriltag_ros/src/tag_bundle_calibration.cpp b/apriltag_ros/src/tag_bundle_calibration.cpp index 98518897..9ea4902e 100644 --- a/apriltag_ros/src/tag_bundle_calibration.cpp +++ b/apriltag_ros/src/tag_bundle_calibration.cpp @@ -2,102 +2,6 @@ namespace apriltag_ros { - -namespace detail -{ -geometry_msgs::Pose averagePoses(const std::vector& poses) -{ - geometry_msgs::Pose ret; - - if (poses.empty()) - return ret; - - // Averaging positions - double sz = static_cast(poses.size()); - geometry_msgs::Point init_position; - geometry_msgs::Point avg_position = - std::accumulate(poses.begin(), - poses.end(), - init_position, - [sz](geometry_msgs::Point& accum, const geometry_msgs::Pose& curr) - { - accum.x += curr.position.x / sz; - accum.y += curr.position.y / sz; - accum.z += curr.position.z / sz; - return accum; - }); - - ret.position = avg_position; - - // Averaging orientations - // https://math.stackexchange.com/questions/61146/averaging-quaternions/3435296#3435296 - tf2::Quaternion init_quat{0.0, 0.0, 0.0, 0.0}; - tf2::Quaternion first_quat, curr_quat; - tf2::fromMsg(poses[0].orientation, first_quat); - - auto avg_quat_approx = std::accumulate( - poses.begin(), - poses.end(), - init_quat, - [&first_quat, &curr_quat](tf2::Quaternion& accum, const geometry_msgs::Pose& curr) - { - tf2::fromMsg(curr.orientation, curr_quat); - double weight = curr_quat.dot(first_quat) > 0.0 ? 1.0 : -1.0; - return accum + tf2::Quaternion(weight * curr_quat.x(), - weight * curr_quat.y(), - weight * curr_quat.z(), - weight * curr_quat.w()); - }); - - ret.orientation = tf2::toMsg(avg_quat_approx.normalize()); - - return ret; -} - -void writeToYaml(const std::unordered_map& tag_poses_in_master_frame, - const std::unordered_map& tag_size_map, - const std::string& tag_bundle_name, - std::ostream& os) -{ - // Create a vector of constant references to the elements in tag_poses_in_master_frame - std::vector>> sorted_tags( - tag_poses_in_master_frame.begin(), tag_poses_in_master_frame.end()); - - // Sort the vector by ascending tag ID - std::sort(sorted_tags.begin(), - sorted_tags.end(), - [](const auto& a, const auto& b) { return a.get().first < b.get().first; }); - - os << "standalone_tags:\n"; - os << " [\n"; - os << " ]\n"; - os << "tag_bundles:\n"; - os << " [\n"; - os << " {\n"; - os << " name: '" << tag_bundle_name << "',\n"; - os << " layout:\n"; - os << " [\n"; - - for (const auto& tag_entry_ref : sorted_tags) - { - const auto& tag_entry = tag_entry_ref.get(); - int tag_id = tag_entry.first; - const geometry_msgs::Pose& pose = tag_entry.second; - double size = tag_size_map.at(tag_id); - os << " {id: " << tag_id << ", size: " << size << ", x: " << pose.position.x - << ", y: " << pose.position.y << ", z: " << pose.position.z << ", qw: " << pose.orientation.w - << ", qx: " << pose.orientation.x << ", qy: " << pose.orientation.y - << ", qz: " << pose.orientation.z << "},\n"; - } - - os << " ]\n"; - os << " }\n"; - os << " ]\n"; -} - -} // namespace detail - -// TagBundleCalibrationNode class TagBundleCalibrationNode::TagBundleCalibrationNode(int max_detections, const std::string& config_file_path, const std::string& tag_bundle_name, @@ -196,9 +100,51 @@ void TagBundleCalibrationNode::tagDetectionCallback( ros::shutdown(); return; } - detail::writeToYaml(tags_in_master_frame_, tag_size_map_, tag_bundle_name_, ofs); + writeToYaml(tags_in_master_frame_, tag_size_map_, tag_bundle_name_, ofs); ros::shutdown(); } } +void TagBundleCalibrationNode::writeToYaml( + const std::unordered_map& tag_poses_in_master_frame, + const std::unordered_map& tag_size_map, + const std::string& tag_bundle_name, + std::ostream& os) +{ + // Create a vector of constant references to the elements in tag_poses_in_master_frame + std::vector>> sorted_tags( + tag_poses_in_master_frame.begin(), tag_poses_in_master_frame.end()); + + // Sort the vector by ascending tag ID + std::sort(sorted_tags.begin(), + sorted_tags.end(), + [](const auto& a, const auto& b) { return a.get().first < b.get().first; }); + + os << "standalone_tags:\n"; + os << " [\n"; + os << " ]\n"; + os << "tag_bundles:\n"; + os << " [\n"; + os << " {\n"; + os << " name: '" << tag_bundle_name << "',\n"; + os << " layout:\n"; + os << " [\n"; + + for (const auto& tag_entry_ref : sorted_tags) + { + const auto& tag_entry = tag_entry_ref.get(); + int tag_id = tag_entry.first; + const geometry_msgs::Pose& pose = tag_entry.second; + double size = tag_size_map.at(tag_id); + os << " {id: " << tag_id << ", size: " << size << ", x: " << pose.position.x + << ", y: " << pose.position.y << ", z: " << pose.position.z << ", qw: " << pose.orientation.w + << ", qx: " << pose.orientation.x << ", qy: " << pose.orientation.y + << ", qz: " << pose.orientation.z << "},\n"; + } + + os << " ]\n"; + os << " }\n"; + os << " ]\n"; +} + } // namespace apriltag_ros From 1871c8c06f51777f09fbbdd61497210924b2bc6d Mon Sep 17 00:00:00 2001 From: Jack Zender Date: Fri, 21 Apr 2023 14:27:49 +0000 Subject: [PATCH 5/8] const correctness and log message on first callback --- apriltag_ros/include/apriltag_ros/tag_bundle_calibration.h | 2 +- apriltag_ros/src/tag_bundle_calibration.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/apriltag_ros/include/apriltag_ros/tag_bundle_calibration.h b/apriltag_ros/include/apriltag_ros/tag_bundle_calibration.h index 72a55330..9a1a98b0 100644 --- a/apriltag_ros/include/apriltag_ros/tag_bundle_calibration.h +++ b/apriltag_ros/include/apriltag_ros/tag_bundle_calibration.h @@ -32,7 +32,7 @@ class TagBundleCalibrationNode void writeToYaml(const std::unordered_map& tag_poses_in_master_frame, const std::unordered_map& tag_size_map, const std::string& tag_bundle_name, - std::ostream& os); + std::ostream& os) const; ros::Subscriber tag_detection_sub_; int max_detections_; diff --git a/apriltag_ros/src/tag_bundle_calibration.cpp b/apriltag_ros/src/tag_bundle_calibration.cpp index 9ea4902e..d7a4ef4b 100644 --- a/apriltag_ros/src/tag_bundle_calibration.cpp +++ b/apriltag_ros/src/tag_bundle_calibration.cpp @@ -27,6 +27,7 @@ TagBundleCalibrationNode::TagBundleCalibrationNode(int max_detections, void TagBundleCalibrationNode::tagDetectionCallback( const apriltag_ros::AprilTagDetectionArray::ConstPtr& msg) { + ROS_INFO_ONCE("Processing tag detections..."); std::unordered_map curr_tag_poses_in_cam_frame; for (const auto& tag_detection : msg->detections) @@ -109,7 +110,7 @@ void TagBundleCalibrationNode::writeToYaml( const std::unordered_map& tag_poses_in_master_frame, const std::unordered_map& tag_size_map, const std::string& tag_bundle_name, - std::ostream& os) + std::ostream& os) const { // Create a vector of constant references to the elements in tag_poses_in_master_frame std::vector>> sorted_tags( From dc4096308f1e136648aa753dc9bfb5a6f36b8107 Mon Sep 17 00:00:00 2001 From: Jack Zender Date: Fri, 21 Apr 2023 14:29:25 +0000 Subject: [PATCH 6/8] remove some comments --- apriltag_ros/src/apriltag_ros_tag_bundle_calibration_node.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/apriltag_ros/src/apriltag_ros_tag_bundle_calibration_node.cpp b/apriltag_ros/src/apriltag_ros_tag_bundle_calibration_node.cpp index 97986049..f75205a1 100644 --- a/apriltag_ros/src/apriltag_ros_tag_bundle_calibration_node.cpp +++ b/apriltag_ros/src/apriltag_ros_tag_bundle_calibration_node.cpp @@ -3,11 +3,10 @@ int main(int argc, char** argv) { - // Initialize the node and create a NodeHandle ros::init(argc, argv, "tag_bundle_calibration_node"); ros::NodeHandle nh{"~"}; - // Read the parameters + // Read params int max_detections; std::string config_file_path; std::string tag_bundle_name; @@ -17,7 +16,6 @@ int main(int argc, char** argv) nh.param("tag_bundle_name", tag_bundle_name, std::string("tag_bundle")); nh.param("master_tag_id", master_tag_id, 0); - // Create the node apriltag_ros::TagBundleCalibrationNode node( max_detections, config_file_path, tag_bundle_name, master_tag_id); From d1dc8e0f298585d725793843a8d26d4b01e1e74e Mon Sep 17 00:00:00 2001 From: Jack Zender Date: Tue, 25 Apr 2023 15:18:49 +0000 Subject: [PATCH 7/8] logging of progress as well as making sure not to count empty detections and tag bundles --- apriltag_ros/src/tag_bundle_calibration.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/apriltag_ros/src/tag_bundle_calibration.cpp b/apriltag_ros/src/tag_bundle_calibration.cpp index d7a4ef4b..77623ea5 100644 --- a/apriltag_ros/src/tag_bundle_calibration.cpp +++ b/apriltag_ros/src/tag_bundle_calibration.cpp @@ -27,11 +27,17 @@ TagBundleCalibrationNode::TagBundleCalibrationNode(int max_detections, void TagBundleCalibrationNode::tagDetectionCallback( const apriltag_ros::AprilTagDetectionArray::ConstPtr& msg) { + if (msg->detections.empty()) + return; + ROS_INFO_ONCE("Processing tag detections..."); std::unordered_map curr_tag_poses_in_cam_frame; for (const auto& tag_detection : msg->detections) { + // Don't try to calibrate on tag bundle detections + if (tag_detection.id.size() > 1) + return; int tag_id = tag_detection.id[0]; double tag_size = tag_detection.size[0]; geometry_msgs::Pose tag_pose_in_master_frame = tag_detection.pose.pose.pose; @@ -92,6 +98,9 @@ void TagBundleCalibrationNode::tagDetectionCallback( received_detections_++; + ROS_INFO_STREAM_THROTTLE(1.0, received_detections_ << " of " << max_detections_ + << " detections complete."); + if (received_detections_ >= max_detections_) { std::ofstream ofs{config_file_path_}; From 97c5f9c5ca558e5d5f47576fb29f8729fbd23d1d Mon Sep 17 00:00:00 2001 From: Jack Zender Date: Tue, 25 Apr 2023 15:22:20 +0000 Subject: [PATCH 8/8] make sure to set the origin quat correctly --- apriltag_ros/src/tag_bundle_calibration.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/apriltag_ros/src/tag_bundle_calibration.cpp b/apriltag_ros/src/tag_bundle_calibration.cpp index 77623ea5..cfcca18f 100644 --- a/apriltag_ros/src/tag_bundle_calibration.cpp +++ b/apriltag_ros/src/tag_bundle_calibration.cpp @@ -58,8 +58,11 @@ void TagBundleCalibrationNode::tagDetectionCallback( tf2::fromMsg(master_tag_pose, master_tag_tf); // init the master tag pose if it is not already in the map - if (tags_in_master_frame_.find(master_tag_id_) == tags_in_master_frame_.end()) - tags_in_master_frame_[master_tag_id_] = geometry_msgs::Pose{}; + if (tags_in_master_frame_.find(master_tag_id_) == tags_in_master_frame_.end()) { + geometry_msgs::Pose origin{}; + origin.orientation.w = 1.0; + tags_in_master_frame_[master_tag_id_] = origin; + } for (auto& tag_pose_kv : curr_tag_poses_in_cam_frame) {