calibration: fix hardcoded number of tags -> use value from config

include/basalt/calibration/calibration_helper.h

@@ -78,6 +78,7 @@ using CalibInitPoseMap =
 class CalibHelper {
  public:
   static void detectCorners(const VioDatasetPtr& vio_data,
+                            const AprilGrid& april_grid,
                             CalibCornerMap& calib_corners,
                             CalibCornerMap& calib_corners_rejected);
 

src/calibration/calibraiton_helper.cpp

@@ -106,6 +106,7 @@ bool estimateTransformation(
 }
 
 void CalibHelper::detectCorners(const VioDatasetPtr &vio_data,
+                                const AprilGrid &april_grid,
                                 CalibCornerMap &calib_corners,
                                 CalibCornerMap &calib_corners_rejected) {
   calib_corners.clear();
@@ -114,7 +115,8 @@ void CalibHelper::detectCorners(const VioDatasetPtr &vio_data,
   tbb::parallel_for(
       tbb::blocked_range<size_t>(0, vio_data->get_image_timestamps().size()),
       [&](const tbb::blocked_range<size_t> &r) {
-        ApriltagDetector ad;
+        const int numTags = april_grid.getTagCols() * april_grid.getTagRows();
+        ApriltagDetector ad(numTags);
 
         for (size_t j = r.begin(); j != r.end(); ++j) {
           int64_t timestamp_ns = vio_data->get_image_timestamps()[j];

src/calibration/cam_calib.cpp

@@ -415,7 +415,8 @@ void CamCalib::detectCorners() {
   processing_thread.reset(new std::thread([this]() {
     std::cout << "Started detecting corners" << std::endl;
 
-    CalibHelper::detectCorners(this->vio_dataset, this->calib_corners,
+    CalibHelper::detectCorners(this->vio_dataset, this->april_grid,
+                               this->calib_corners,
                                this->calib_corners_rejected);
 
     std::string path =

src/calibration/cam_imu_calib.cpp

@@ -241,7 +241,8 @@ void CamImuCalib::detectCorners() {
   processing_thread.reset(new std::thread([this]() {
     std::cout << "Started detecting corners" << std::endl;
 
-    CalibHelper::detectCorners(this->vio_dataset, this->calib_corners,
+    CalibHelper::detectCorners(this->vio_dataset, this->april_grid,
+                               this->calib_corners,
                                this->calib_corners_rejected);
 
     std::string path =

thirdparty/apriltag/include/basalt/utils/apriltag.h

@@ -9,7 +9,7 @@ struct ApriltagDetectorData;
 
 class ApriltagDetector {
  public:
-  ApriltagDetector();
+  ApriltagDetector(int numTags);
 
   ~ApriltagDetector();
 

thirdparty/apriltag/src/apriltag.cpp

@@ -8,13 +8,14 @@
 namespace basalt {
 
 struct ApriltagDetectorData {
-  ApriltagDetectorData()
+  ApriltagDetectorData(int numTags)
       : doSubpixRefinement(true),
         maxSubpixDisplacement(0),
         minTagsForValidObs(4),
         minBorderDistance(4.0),
         blackTagBorder(2),
-        _tagCodes(AprilTags::tagCodes36h11) {
+        _tagCodes(AprilTags::tagCodes36h11),
+        _numTags(numTags) {
     _tagDetector =
         std::make_shared<AprilTags::TagDetector>(_tagCodes, blackTagBorder);
   }
@@ -30,10 +31,14 @@ struct ApriltagDetectorData {
   AprilTags::TagCodes _tagCodes;
   std::shared_ptr<AprilTags::TagDetector> _tagDetector;
 
-  inline int size() { return 36 * 4; }
+  int _numTags;  //!< number of tags in the grid (determines the valid ids)
+
+  inline int size() { return _numTags * 4; }
 };
 
-ApriltagDetector::ApriltagDetector() { data = new ApriltagDetectorData; }
+ApriltagDetector::ApriltagDetector(int numTags) {
+  data = new ApriltagDetectorData(numTags);
+}
 
 ApriltagDetector::~ApriltagDetector() { delete data; }