Merge branch 'demmeln/fix-hardcoded-aprilgrid-size' into 'master'

fix hardcoded aprilgrid size

See merge request VladyslavUsenko/basalt!6

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 =

test/src/test_patch.cpp

@@ -26,6 +26,8 @@ struct SmoothFunction {
   BASALT_HOST_DEVICE inline bool InBounds(
       const Eigen::MatrixBase<Derived>& p,
       const typename Derived::Scalar border) const {
+    UNUSED(p);
+    UNUSED(border);
     return true;
   }
 };

thirdparty/apriltag/CMakeLists.txt

@@ -17,7 +17,7 @@ include_directories(../../include)
 include_directories(../basalt-headers/include)
 include_directories(../basalt-headers/thirdparty/Sophus)
 
-add_library(apriltag STATIC ${APRILTAG_SRCS} src/apriltag.cpp)
+add_library(apriltag STATIC ${APRILTAG_SRCS} include/basalt/utils/apriltag.h src/apriltag.cpp)
 
 target_include_directories(apriltag PUBLIC include)
 target_link_libraries(apriltag PUBLIC basalt::opencv Sophus::Sophus)

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; }