Created a node called apriltag_ros_tag_bundle_calibration_node that consumes a remap-able ~tag_detections feed until it reaches max_detections. You can set the master tag with param master_tag_id. It then averages the observed transforms to the master tag id over the duration of the observations and writes the generated config file to config_file_path. We average the transforms' quats using tf2::Quaternion::slerp() which seems to perform adequately.
Open questions
License headers on new files
Unable to test against matlab script as ground truth (namely to compare slerp() to the home-brewed quat averaging solution)
Successive Work
I would like to modify the code to allow transform chaining, so that the master tag id doesn't always have to be in frame
Demo
In the attached demo, we run a tag detector on a 3 x 3 tag grid with tag sizes 3cm and spacing between tags 4.5cm center to center along x and y. Camera is running at 15hz 720p rectified. 500 max detections. Would likely get better results at a higher resolution and higher max detections. This demo had largest errors on the z axis (max error was 7mm for tag 8) Whereas x and y errors were all sub mm. I will likely do some more testing with more detections and higher resolution (and flatter targets i.e not tags taped to the wall).
Summary of Solution
Created a node called
apriltag_ros_tag_bundle_calibration_nodethat consumes a remap-able~tag_detectionsfeed until it reachesmax_detections. You can set the master tag with parammaster_tag_id. It then averages the observed transforms to the master tag id over the duration of the observations and writes the generated config file toconfig_file_path. We average the transforms' quats usingtf2::Quaternion::slerp()which seems to perform adequately.Open questions
slerp()to the home-brewed quat averaging solution)Successive Work
Demo
In the attached demo, we run a tag detector on a 3 x 3 tag grid with tag sizes 3cm and spacing between tags 4.5cm center to center along x and y. Camera is running at 15hz 720p rectified. 500 max detections. Would likely get better results at a higher resolution and higher max detections. This demo had largest errors on the z axis (max error was 7mm for tag 8) Whereas x and y errors were all sub mm. I will likely do some more testing with more detections and higher resolution (and flatter targets i.e not tags taped to the wall).
https://user-images.githubusercontent.com/109549970/233659739-910eac3a-6331-4358-84ad-fa4fce9d7612.mp4