Fix charuco board detection bug (#96)

Author: BAILOOL

McCalib/include/Board.hpp

@@ -49,7 +49,8 @@ class Board final {
   // Functions
   Board() = delete;
   ~Board(){};
-  Board(const std::filesystem::path &config, const int board_idx);
+  Board(const std::filesystem::path &config, const int board_idx,
+        const cv::Ptr<cv::aruco::CharucoBoard> charuco_board);
   void insertNewBoard(const std::shared_ptr<BoardObs> new_board);
   void insertNewFrame(const std::shared_ptr<Frame> new_frame);
 };

McCalib/src/Board.cpp

@@ -19,7 +19,8 @@ namespace McCalib {
  * @param config_path path to the configuration file
  * @param board_idx index of the board
  */
-Board::Board(const std::filesystem::path &config_path, const int board_idx) {
+Board::Board(const std::filesystem::path &config_path, const int board_idx,
+             const cv::Ptr<cv::aruco::CharucoBoard> charuco_board) {
   std::vector<int> number_x_square_per_board, number_y_square_per_board;
   std::vector<double> square_size_per_board;
   std::vector<int> boards_index;
@@ -59,6 +60,19 @@ Board::Board(const std::filesystem::path &config_path, const int board_idx) {
   }
 
   fs.release(); // close the input file
+
+  nb_pts_ = (nb_x_square_ - 1) * (nb_y_square_ - 1);
+
+  // Prepare the 3D pts of the boards
+  pts_3d_.reserve(nb_pts_);
+  for (int y = 0; y < nb_y_square_ - 1; y++) {
+    for (int x = 0; x < nb_x_square_ - 1; x++) {
+      pts_3d_.emplace_back(x * square_size_, y * square_size_, 0);
+    }
+  }
+
+  charuco_board_ = charuco_board;
+
   board_id_ = board_idx;
 
   // initialize color of the board

McCalib/src/McCalib.cpp

@@ -119,25 +119,18 @@ Calibration::Calibration(const std::filesystem::path &config_path) {
   }
 
   const auto charuco_boards = createCharucoBoards(
-      nb_board_, number_x_square_per_board_, number_y_square_per_board_,
+      max_board_idx + 1, number_x_square_per_board_, number_y_square_per_board_,
       length_square, length_marker, dict_);
 
-  // Initialize the 3D boards
-  for (auto const &[i, charuco_board] : charuco_boards) {
-    // Initialize board
-    std::shared_ptr<Board> new_board = std::make_shared<Board>(config_path, i);
-    boards_3d_[i] = new_board;
-    boards_3d_[i]->nb_pts_ =
-        (boards_3d_[i]->nb_x_square_ - 1) * (boards_3d_[i]->nb_y_square_ - 1);
-    boards_3d_[i]->charuco_board_ = charuco_board;
-    boards_3d_[i]->pts_3d_.reserve(boards_3d_[i]->nb_pts_);
-    // Prepare the 3D pts of the boards
-    for (int y = 0; y < boards_3d_[i]->nb_y_square_ - 1; y++) {
-      for (int x = 0; x < boards_3d_[i]->nb_x_square_ - 1; x++) {
-        boards_3d_[i]->pts_3d_.emplace_back(x * boards_3d_[i]->square_size_,
-                                            y * boards_3d_[i]->square_size_, 0);
-      }
-    }
+  for (unsigned int board_idx = 0u; board_idx < nb_board_; ++board_idx) {
+    const int charuco_board_idx =
+        boards_index.size() > 0 ? boards_index[board_idx] : board_idx;
+    assert(charuco_boards.count(charuco_board_idx) > 0);
+    const cv::Ptr<cv::aruco::CharucoBoard> charuco_board =
+        charuco_boards.at(charuco_board_idx);
+
+    boards_3d_[board_idx] =
+        std::make_shared<Board>(config_path, board_idx, charuco_board);
   }
 }
 

tests/test_calibration.cpp

@@ -11,9 +11,15 @@
 
 #define PI 3.14159265
 
-constexpr double INTRINSICS_TOLERANCE = 4.0;          // in percentage
+constexpr double INTRINSICS_TOLERANCE = 4.0;     // in percentage
+constexpr double ROTATION_ERROR_TOLERANCE = 1.0; // in degrees
+
+#if (defined(CV_VERSION_MAJOR) && CV_VERSION_MAJOR <= 4 &&                     \
+     defined(CV_VERSION_MINOR) && CV_VERSION_MINOR < 7)
 constexpr double TRANSLATION_ERROR_TOLERANCE = 0.005; // in meters
-constexpr double ROTATION_ERROR_TOLERANCE = 1.0;      // in degrees
+#else
+constexpr double TRANSLATION_ERROR_TOLERANCE = 0.01; // in meters
+#endif
 
 double getTranslationError(cv::Mat a, cv::Mat b) {
   double dist = cv::norm(a, b, cv::NORM_L2);