Tag bundle calibration

.gitignore

@@ -2,3 +2,4 @@
 **/*.bag
 .vscode/
 build/
+.clang-format

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

apriltag_ros/include/apriltag_ros/tag_bundle_calibration.h

@@ -0,0 +1,49 @@
+#ifndef APRILTAG_ROS_TAG_BUNDLE_CALIBRATION_H
+#define APRILTAG_ROS_TAG_BUNDLE_CALIBRATION_H
+
+#include <apriltag_ros/AprilTagDetection.h>
+#include <apriltag_ros/AprilTagDetectionArray.h>
+
+#include <geometry_msgs/Pose.h>
+#include <ros/ros.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Transform.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+#include <fstream>
+#include <iostream>
+#include <numeric>
+#include <unordered_map>
+#include <vector>
+
+namespace apriltag_ros
+{
+class TagBundleCalibrationNode
+{
+public:
+  TagBundleCalibrationNode(int max_detections,
+                           const std::string& config_file_path,
+                           const std::string& tag_bundle_name,
+                           int master_tag_id);
+
+private:
+  void tagDetectionCallback(const apriltag_ros::AprilTagDetectionArray::ConstPtr& msg);
+
+  void writeToYaml(const std::unordered_map<int, geometry_msgs::Pose>& tag_poses_in_master_frame,
+                   const std::unordered_map<int, double>& tag_size_map,
+                   const std::string& tag_bundle_name,
+                   std::ostream& os) const;
+
+  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<int, geometry_msgs::Pose> tags_in_master_frame_;
+  std::unordered_map<int, double> tag_size_map_;
+};
+
+} // namespace apriltag_ros
+
+#endif // APRILTAG_ROS_TAG_BUNDLE_CALIBRATION_H

apriltag_ros/launch/tag_bundle_calibration.launch

@@ -0,0 +1,15 @@
+<launch>    
+    <arg name="tag_detection_topic" default="/tag_detections"/>
+    <arg name="max_detections" default="500"/>
+    <arg name="config_file_path" default="/tmp/tag_bundle_calibration.yaml"/>
+    <arg name="tag_bundle_name" default="tag_bundle"/>
+    <arg name="master_tag_id" default="0"/>
+
+    <node pkg="apriltag_ros" type="apriltag_ros_tag_bundle_calibration_node" name="apriltag_ros_tag_bundle_calibration_node" output="screen">
+        <remap from="~tag_detections" to="$(arg tag_detection_topic)"/>
+        <param name="max_detections" value="$(arg max_detections)"/>
+        <param name="config_file_path" value="$(arg config_file_path)"/>
+        <param name="tag_bundle_name" value="$(arg tag_bundle_name)"/>
+        <param name="master_tag_id" value="$(arg master_tag_id)"/>
+    </node>
+</launch>

apriltag_ros/package.xml

@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<package>
+<package format="2">
   <name>apriltag_ros</name>
   <version>3.2.1</version>
   <description>
@@ -26,37 +26,25 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
-  <build_depend>apriltag</build_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>image_transport</build_depend>
-  <build_depend>image_geometry</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>sensor_msgs</build_depend>
-  <build_depend>message_generation</build_depend>
-  <build_depend>std_msgs</build_depend>
-  <build_depend>cv_bridge</build_depend>
-  <build_depend>tf</build_depend>
-  <build_depend>nodelet</build_depend>
-  <build_depend>pluginlib</build_depend>
-  <build_depend>eigen</build_depend>
-  <build_depend>libopencv-dev</build_depend>
-  <build_depend>cmake_modules</build_depend>
+  <depend>apriltag</depend>
+  <depend>geometry_msgs</depend>
+  <depend>image_transport</depend>
+  <depend>image_geometry</depend>
+  <depend>roscpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>message_generation</depend>
+  <depend>message_runtime</depend>
+  <depend>std_msgs</depend>
+  <depend>cv_bridge</depend>
+  <depend>tf</depend>
+  <depend>nodelet</depend>
+  <depend>pluginlib</depend>
+  <depend>eigen</depend>
+  <depend>libopencv-dev</depend>
+  <depend>cmake_modules</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
 
-  <run_depend>tf</run_depend>
-  <run_depend>apriltag</run_depend>
-  <run_depend>geometry_msgs</run_depend>
-  <run_depend>image_transport</run_depend>
-  <run_depend>image_geometry</run_depend>
-  <run_depend>roscpp</run_depend>
-  <run_depend>sensor_msgs</run_depend>
-  <run_depend>message_runtime</run_depend>
-  <run_depend>std_msgs</run_depend>
-  <run_depend>cv_bridge</run_depend>
-  <run_depend>nodelet</run_depend>
-  <run_depend>pluginlib</run_depend>
-  <run_depend>eigen</run_depend>
-  <run_depend>libopencv-dev</run_depend>
-
   <export>
     <nodelet plugin="${prefix}/nodelet_plugins.xml" />
   </export>

apriltag_ros/src/apriltag_ros_tag_bundle_calibration_node.cpp

@@ -0,0 +1,24 @@
+#include "apriltag_ros/tag_bundle_calibration.h"
+#include <ros/ros.h>
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "tag_bundle_calibration_node");
+  ros::NodeHandle nh{"~"};
+
+  // Read params
+  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);
+
+  apriltag_ros::TagBundleCalibrationNode node(
+      max_detections, config_file_path, tag_bundle_name, master_tag_id);
+
+  ros::spin();
+  return 0;
+}

apriltag_ros/src/tag_bundle_calibration.cpp

@@ -0,0 +1,163 @@
+#include <apriltag_ros/tag_bundle_calibration.h>
+
+namespace apriltag_ros
+{
+TagBundleCalibrationNode::TagBundleCalibrationNode(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)
+{
+  ros::NodeHandle pnh{"~"};
+  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)
+{
+  if (msg->detections.empty())
+    return;
+
+  ROS_INFO_ONCE("Processing tag detections...");
+  std::unordered_map<int, geometry_msgs::Pose> 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;
+
+    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()) {
+    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)
+  {
+    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
+    {
+      // 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);
+    }
+  }
+
+  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_};
+    if (!ofs.is_open())
+    {
+      ROS_ERROR_STREAM("Failed to open file: " << config_file_path_);
+      ros::shutdown();
+      return;
+    }
+    writeToYaml(tags_in_master_frame_, tag_size_map_, tag_bundle_name_, ofs);
+    ros::shutdown();
+  }
+}
+
+void TagBundleCalibrationNode::writeToYaml(
+    const std::unordered_map<int, geometry_msgs::Pose>& tag_poses_in_master_frame,
+    const std::unordered_map<int, double>& tag_size_map,
+    const std::string& tag_bundle_name,
+    std::ostream& os) const
+{
+  // Create a vector of constant references to the elements in tag_poses_in_master_frame
+  std::vector<std::reference_wrapper<const std::pair<const int, geometry_msgs::Pose>>> 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