fix opencv init intrinsic calib (#27)

src/Camera.cpp

@@ -190,15 +190,22 @@ void Camera::initializeCalibration() {
     std::shared_ptr<BoardObs> board_obs_temp =
         board_observations_[indbv[shuffled_board_ind[i]]].lock();
     if (board_obs_temp) {
-      img_points.emplace_back(board_obs_temp->pts_2d_);
-      const std::vector<int> &corners_idx_temp = board_obs_temp->charuco_id_;
-      std::shared_ptr<Board> board_3d_ptr = board_obs_temp->board_3d_.lock();
-      if (board_3d_ptr) {
-        std::vector<cv::Point3f> pts_3d_temp;
-        pts_3d_temp.reserve(corners_idx_temp.size());
-        for (const auto &corner_idx_temp : corners_idx_temp)
-          pts_3d_temp.emplace_back(board_3d_ptr->pts_3d_[corner_idx_temp]);
-        obj_points.emplace_back(pts_3d_temp);
+      // if fisheye, check if points are not too close to borders
+      bool valid_board = true;
+      if (distortion_model_ == 1) {
+        valid_board = checkBorderToleranceFisheye(board_obs_temp);
+      }
+      if (valid_board == true) {
+        img_points.emplace_back(board_obs_temp->pts_2d_);
+        const std::vector<int> &corners_idx_temp = board_obs_temp->charuco_id_;
+        std::shared_ptr<Board> board_3d_ptr = board_obs_temp->board_3d_.lock();
+        if (board_3d_ptr) {
+          std::vector<cv::Point3f> pts_3d_temp;
+          pts_3d_temp.reserve(corners_idx_temp.size());
+          for (const auto &corner_idx_temp : corners_idx_temp)
+            pts_3d_temp.emplace_back(board_3d_ptr->pts_3d_[corner_idx_temp]);
+          obj_points.emplace_back(pts_3d_temp);
+        }
       }
     }
   }
@@ -225,6 +232,29 @@ void Camera::initializeCalibration() {
   setIntrinsics(camera_matrix, distortion_coeffs);
 }
 
+/**
+ * @brief determine if the board is valid for the calibration
+ * of fisheye cameras
+ * bug fix opencv from:
+ * https://github.com/realizator/stereopi-fisheye-robot/blob/master/
+ * 4_calibration_fisheye.py
+ */
+bool Camera::checkBorderToleranceFisheye(std::shared_ptr<BoardObs> board_obs) {
+  bool valid_board = true;
+  const float thresh_border_x = float(im_cols_) * border_marging;
+  const float thresh_border_y = float(im_rows_) * border_marging;
+  for (int i = 0; i < board_obs->pts_2d_.size(); i++) {
+    const float &pt_x = board_obs->pts_2d_[i].x;
+    const float &pt_y = board_obs->pts_2d_[i].y;
+    if (pt_x <= thresh_border_x || pt_x >= (im_cols_ - thresh_border_x) ||
+        pt_y <= thresh_border_y || pt_y >= (im_rows_ - thresh_border_y)) {
+      valid_board = false;
+      break;
+    }
+  }
+  return valid_board;
+}
+
 /**
  * @brief Refinement of the camera parameters of the current camera
  *

src/Camera.hpp

@@ -22,6 +22,11 @@
  * - object observation
  */
 class Camera final {
+
+private:
+  // fisheye border margin to exclude invalid boards during initialization
+  const float border_marging = 0.05f; // border margin tolerance
+
 public:
   // datastructure for this camera
   std::map<int, std::weak_ptr<BoardObs>>
@@ -59,4 +64,5 @@ class Camera final {
   cv::Mat getDistortionVectorVector() const;
   void getIntrinsics(cv::Mat &K, cv::Mat &distortion_vector);
   void setIntrinsics(const cv::Mat K, const cv::Mat distortion_vector);
+  bool checkBorderToleranceFisheye(std::shared_ptr<BoardObs> board_obs);
 };