Parametrize grid size for keypt_indices_in_cells_ (#584)

example/aist/equirectangular.yaml

@@ -51,3 +51,5 @@ GlobalOptimizer:
 
 System:
   map_format: "msgpack"
+  num_grid_cols: 96
+  num_grid_rows: 48

example/aist/fisheye.yaml

@@ -42,3 +42,5 @@ Mapping:
 
 System:
   map_format: "msgpack"
+  num_grid_cols: 48
+  num_grid_rows: 27

example/euroc/EuRoC_mono.yaml

@@ -56,6 +56,8 @@ LoopDetector:
 
 System:
   map_format: "msgpack"
+  num_grid_cols: 47
+  num_grid_rows: 30
 
 PangolinViewer:
   keyframe_size: 0.07

example/euroc/EuRoC_stereo.yaml

@@ -102,6 +102,8 @@ Initializer:
 
 System:
   map_format: "msgpack"
+  num_grid_cols: 47
+  num_grid_rows: 30
 
 PangolinViewer:
   keyframe_size: 0.07

example/kitti/KITTI_mono_00-02.yaml

@@ -1,5 +1,9 @@
 # KITTI monocular model for seq.00-02
 
+System:
+  num_grid_cols: 40
+  num_grid_rows: 13
+
 #==============#
 # Camera Model #
 #==============#

example/kitti/KITTI_mono_03.yaml

@@ -1,5 +1,9 @@
 # KITTI monocular model for seq.03
 
+System:
+  num_grid_cols: 40
+  num_grid_rows: 13
+
 #==============#
 # Camera Model #
 #==============#

example/kitti/KITTI_mono_04-12.yaml

@@ -1,5 +1,9 @@
 # KITTI monocular model for seq.04-12
 
+System:
+  num_grid_cols: 40
+  num_grid_rows: 13
+
 #==============#
 # Camera Model #
 #==============#

example/kitti/KITTI_stereo_00-02.yaml

@@ -1,5 +1,9 @@
 # KITTI stereo model for seq.00-02
 
+System:
+  num_grid_cols: 40
+  num_grid_rows: 13
+
 #==============#
 # Camera Model #
 #==============#

example/kitti/KITTI_stereo_03.yaml

@@ -1,5 +1,9 @@
 # KITTI stereo model for seq.03
 
+System:
+  num_grid_cols: 40
+  num_grid_rows: 13
+
 #==============#
 # Camera Model #
 #==============#

example/kitti/KITTI_stereo_04-12.yaml

@@ -1,5 +1,9 @@
 # KITTI stereo model for seq.04-12
 
+System:
+  num_grid_cols: 40
+  num_grid_rows: 13
+
 #==============#
 # Camera Model #
 #==============#

example/tum_vi/TUM_VI_mono.yaml

@@ -2,6 +2,10 @@
 # https://vision.in.tum.de/data/datasets/visual-inertial-dataset#geometric_calibration
 # pinhole (512x512)
 
+System:
+  num_grid_cols: 16
+  num_grid_rows: 16
+
 #==============#
 # Camera Model #
 #==============#

example/tum_vi/TUM_VI_stereo.yaml

@@ -2,6 +2,10 @@
 # https://vision.in.tum.de/data/datasets/visual-inertial-dataset#geometric_calibration
 # pinhole (512x512)
 
+System:
+  num_grid_cols: 16
+  num_grid_rows: 16
+
 #==============#
 # Camera Model #
 #==============#

src/stella_vslam/camera/base.cc

@@ -10,12 +10,10 @@ namespace camera {
 
 base::base(const std::string& name, const setup_type_t setup_type, const model_type_t model_type, const color_order_t color_order,
            const unsigned int cols, const unsigned int rows, const double fps,
-           const double focal_x_baseline, const double true_baseline, const double depth_thr,
-           const unsigned int num_grid_cols, const unsigned int num_grid_rows)
+           const double focal_x_baseline, const double true_baseline, const double depth_thr)
     : name_(name), setup_type_(setup_type), model_type_(model_type), color_order_(color_order),
       cols_(cols), rows_(rows), fps_(fps),
-      focal_x_baseline_(focal_x_baseline), true_baseline_(true_baseline), depth_thr_(depth_thr),
-      num_grid_cols_(num_grid_cols), num_grid_rows_(num_grid_rows) {
+      focal_x_baseline_(focal_x_baseline), true_baseline_(true_baseline), depth_thr_(depth_thr) {
     spdlog::debug("CONSTRUCT: camera::base");
 }
 

src/stella_vslam/camera/base.h

@@ -58,8 +58,7 @@ class base {
     //! Constructor
     base(const std::string& name, const setup_type_t setup_type, const model_type_t model_type, const color_order_t color_order,
          const unsigned int cols, const unsigned int rows, const double fps,
-         const double focal_x_baseline, const double true_baseline, const double depth_thr,
-         const unsigned int num_grid_cols = 64, const unsigned int num_grid_rows = 48);
+         const double focal_x_baseline, const double true_baseline, const double depth_thr);
 
     //! Destructor
     virtual ~base();
@@ -120,22 +119,12 @@ class base {
     //! if a stereo-triangulated point is farther than this threshold, it is invalid)
     const double depth_thr_;
 
-    //! number of columns of grid to accelerate reprojection matching
-    const unsigned int num_grid_cols_;
-    //! number of rows of grid to accelerate reprojection matching
-    const unsigned int num_grid_rows_;
-
     //---------------------------
     // To be set in derived classes
 
     //! information of image boundary
     image_bounds img_bounds_;
 
-    //! cell width of grid pattern
-    double inv_cell_width_ = std::numeric_limits<double>::quiet_NaN();
-    //! cell height of grid pattern
-    double inv_cell_height_ = std::numeric_limits<double>::quiet_NaN();
-
     //-------------------------
     // To be implemented in derived classes
 

src/stella_vslam/camera/equirectangular.cc

@@ -12,9 +12,6 @@ equirectangular::equirectangular(const std::string& name, const color_order_t& c
     spdlog::debug("CONSTRUCT: camera::equirectangular");
 
     img_bounds_ = compute_image_bounds();
-
-    inv_cell_width_ = static_cast<double>(num_grid_cols_) / (img_bounds_.max_x_ - img_bounds_.min_x_);
-    inv_cell_height_ = static_cast<double>(num_grid_rows_) / (img_bounds_.max_y_ - img_bounds_.min_y_);
 }
 
 equirectangular::equirectangular(const YAML::Node& yaml_node)
@@ -89,9 +86,7 @@ nlohmann::json equirectangular::to_json() const {
             {"cols", cols_},
             {"rows", rows_},
             {"fps", fps_},
-            {"focal_x_baseline", focal_x_baseline_},
-            {"num_grid_cols", num_grid_cols_},
-            {"num_grid_rows", num_grid_rows_}};
+            {"focal_x_baseline", focal_x_baseline_}};
 }
 
 std::ostream& operator<<(std::ostream& os, const equirectangular& params) {
@@ -103,8 +98,6 @@ std::ostream& operator<<(std::ostream& os, const equirectangular& params) {
     os << "- color: " << params.get_color_order_string() << std::endl;
     os << "- model: " << params.get_model_type_string() << std::endl;
     os << "- focal x baseline: " << params.focal_x_baseline_ << std::endl;
-    os << "- num grid cols: " << params.num_grid_cols_ << std::endl;
-    os << "- num grid rows: " << params.num_grid_rows_ << std::endl;
     return os;
 }
 

src/stella_vslam/camera/fisheye.cc

@@ -25,9 +25,6 @@ fisheye::fisheye(const std::string& name, const setup_type_t& setup_type, const
     eigen_dist_params_ << k1_, k2_, k3_, k4_;
 
     img_bounds_ = compute_image_bounds();
-
-    inv_cell_width_ = static_cast<double>(num_grid_cols_) / (img_bounds_.max_x_ - img_bounds_.min_x_);
-    inv_cell_height_ = static_cast<double>(num_grid_rows_) / (img_bounds_.max_y_ - img_bounds_.min_y_);
 }
 
 fisheye::fisheye(const YAML::Node& yaml_node)
@@ -219,8 +216,6 @@ nlohmann::json fisheye::to_json() const {
             {"rows", rows_},
             {"fps", fps_},
             {"focal_x_baseline", focal_x_baseline_},
-            {"num_grid_cols", num_grid_cols_},
-            {"num_grid_rows", num_grid_rows_},
             {"fx", fx_},
             {"fy", fy_},
             {"cx", cx_},

src/stella_vslam/camera/perspective.cc

@@ -25,9 +25,6 @@ perspective::perspective(const std::string& name, const setup_type_t& setup_type
     eigen_dist_params_ << k1_, k2_, p1_, p2_, k3_;
 
     img_bounds_ = compute_image_bounds();
-
-    inv_cell_width_ = static_cast<double>(num_grid_cols_) / (img_bounds_.max_x_ - img_bounds_.min_x_);
-    inv_cell_height_ = static_cast<double>(num_grid_rows_) / (img_bounds_.max_y_ - img_bounds_.min_y_);
 }
 
 perspective::perspective(const YAML::Node& yaml_node)
@@ -180,8 +177,6 @@ nlohmann::json perspective::to_json() const {
             {"rows", rows_},
             {"fps", fps_},
             {"focal_x_baseline", focal_x_baseline_},
-            {"num_grid_cols", num_grid_cols_},
-            {"num_grid_rows", num_grid_rows_},
             {"fx", fx_},
             {"fy", fy_},
             {"cx", cx_},

src/stella_vslam/camera/radial_division.cc

@@ -24,9 +24,6 @@ radial_division::radial_division(const std::string& name, const setup_type_t& se
     eigen_cam_matrix_ << fx_, 0, cx_, 0, fy_, cy_, 0, 0, 1;
 
     img_bounds_ = compute_image_bounds();
-
-    inv_cell_width_ = static_cast<double>(num_grid_cols_) / (img_bounds_.max_x_ - img_bounds_.min_x_);
-    inv_cell_height_ = static_cast<double>(num_grid_rows_) / (img_bounds_.max_y_ - img_bounds_.min_y_);
 }
 
 radial_division::radial_division(const YAML::Node& yaml_node)
@@ -167,8 +164,6 @@ nlohmann::json radial_division::to_json() const {
         {"rows", rows_},
         {"fps", fps_},
         {"focal_x_baseline", focal_x_baseline_},
-        {"num_grid_cols", num_grid_cols_},
-        {"num_grid_rows", num_grid_rows_},
         {"fx", fx_},
         {"fy", fy_},
         {"cx", cx_},

src/stella_vslam/data/common.cc

@@ -81,13 +81,17 @@ cv::Mat convert_json_to_descriptors(const nlohmann::json& json_descriptors) {
 }
 
 void assign_keypoints_to_grid(const camera::base* camera, const std::vector<cv::KeyPoint>& undist_keypts,
-                              std::vector<std::vector<std::vector<unsigned int>>>& keypt_indices_in_cells) {
+                              std::vector<std::vector<std::vector<unsigned int>>>& keypt_indices_in_cells,
+                              unsigned int num_grid_cols, unsigned int num_grid_rows) {
+    double inv_cell_width = static_cast<double>(num_grid_cols) / (camera->img_bounds_.max_x_ - camera->img_bounds_.min_x_);
+    double inv_cell_height = static_cast<double>(num_grid_rows) / (camera->img_bounds_.max_y_ - camera->img_bounds_.min_y_);
+
     // Pre-allocate memory
     const unsigned int num_keypts = undist_keypts.size();
-    const unsigned int num_to_reserve = 0.5 * num_keypts / (camera->num_grid_cols_ * camera->num_grid_rows_);
-    keypt_indices_in_cells.resize(camera->num_grid_cols_);
+    const unsigned int num_to_reserve = 0.5 * num_keypts / (num_grid_cols * num_grid_rows);
+    keypt_indices_in_cells.resize(num_grid_cols);
     for (auto& keypt_indices_in_row : keypt_indices_in_cells) {
-        keypt_indices_in_row.resize(camera->num_grid_rows_);
+        keypt_indices_in_row.resize(num_grid_rows);
         for (auto& keypt_indices_in_cell : keypt_indices_in_row) {
             keypt_indices_in_cell.reserve(num_to_reserve);
         }
@@ -97,52 +101,61 @@ void assign_keypoints_to_grid(const camera::base* camera, const std::vector<cv::
     for (unsigned int idx = 0; idx < num_keypts; ++idx) {
         const auto& keypt = undist_keypts.at(idx);
         int cell_idx_x, cell_idx_y;
-        if (get_cell_indices(camera, keypt, cell_idx_x, cell_idx_y)) {
+        if (get_cell_indices(camera, keypt, num_grid_cols, num_grid_rows, inv_cell_width, inv_cell_height, cell_idx_x, cell_idx_y)) {
             keypt_indices_in_cells.at(cell_idx_x).at(cell_idx_y).push_back(idx);
         }
     }
 }
 
-auto assign_keypoints_to_grid(const camera::base* camera, const std::vector<cv::KeyPoint>& undist_keypts)
+auto assign_keypoints_to_grid(const camera::base* camera, const std::vector<cv::KeyPoint>& undist_keypts,
+                              unsigned int num_grid_cols, unsigned int num_grid_rows)
     -> std::vector<std::vector<std::vector<unsigned int>>> {
     std::vector<std::vector<std::vector<unsigned int>>> keypt_indices_in_cells;
-    assign_keypoints_to_grid(camera, undist_keypts, keypt_indices_in_cells);
+    assign_keypoints_to_grid(camera, undist_keypts, keypt_indices_in_cells, num_grid_cols, num_grid_rows);
     return keypt_indices_in_cells;
 }
 
 std::vector<unsigned int> get_keypoints_in_cell(const camera::base* camera, const data::frame_observation& frm_obs,
                                                 const float ref_x, const float ref_y, const float margin,
                                                 const int min_level, const int max_level) {
-    return get_keypoints_in_cell(camera, frm_obs.undist_keypts_, frm_obs.keypt_indices_in_cells_, ref_x, ref_y, margin, min_level, max_level);
+    return get_keypoints_in_cell(camera, frm_obs.undist_keypts_, frm_obs.keypt_indices_in_cells_,
+                                 ref_x, ref_y, margin,
+                                 frm_obs.num_grid_cols_, frm_obs.num_grid_rows_,
+                                 min_level, max_level);
 }
 
 std::vector<unsigned int> get_keypoints_in_cell(const camera::base* camera, const std::vector<cv::KeyPoint>& undist_keypts,
                                                 const std::vector<std::vector<std::vector<unsigned int>>>& keypt_indices_in_cells,
                                                 const float ref_x, const float ref_y, const float margin,
+                                                const unsigned int num_grid_cols, const unsigned int num_grid_rows,
                                                 const int min_level, const int max_level) {
+    double inv_cell_width = static_cast<double>(num_grid_cols) / (camera->img_bounds_.max_x_ - camera->img_bounds_.min_x_);
+    double inv_cell_height = static_cast<double>(num_grid_rows) / (camera->img_bounds_.max_y_ - camera->img_bounds_.min_y_);
+
     std::vector<unsigned int> indices;
-    indices.reserve(undist_keypts.size());
 
-    const int min_cell_idx_x = std::max(0, cvFloor((ref_x - camera->img_bounds_.min_x_ - margin) * camera->inv_cell_width_));
-    if (static_cast<int>(camera->num_grid_cols_) <= min_cell_idx_x) {
+    const int min_cell_idx_x = std::max(0, cvFloor((ref_x - camera->img_bounds_.min_x_ - margin) * inv_cell_width));
+    if (static_cast<int>(num_grid_cols) <= min_cell_idx_x) {
         return indices;
     }
 
-    const int max_cell_idx_x = std::min(static_cast<int>(camera->num_grid_cols_ - 1), cvCeil((ref_x - camera->img_bounds_.min_x_ + margin) * camera->inv_cell_width_));
+    const int max_cell_idx_x = std::min(static_cast<int>(num_grid_cols - 1), cvCeil((ref_x - camera->img_bounds_.min_x_ + margin) * inv_cell_width));
     if (max_cell_idx_x < 0) {
         return indices;
     }
 
-    const int min_cell_idx_y = std::max(0, cvFloor((ref_y - camera->img_bounds_.min_y_ - margin) * camera->inv_cell_height_));
-    if (static_cast<int>(camera->num_grid_rows_) <= min_cell_idx_y) {
+    const int min_cell_idx_y = std::max(0, cvFloor((ref_y - camera->img_bounds_.min_y_ - margin) * inv_cell_height));
+    if (static_cast<int>(num_grid_rows) <= min_cell_idx_y) {
         return indices;
     }
 
-    const int max_cell_idx_y = std::min(static_cast<int>(camera->num_grid_rows_ - 1), cvCeil((ref_y - camera->img_bounds_.min_y_ + margin) * camera->inv_cell_height_));
+    const int max_cell_idx_y = std::min(static_cast<int>(num_grid_rows - 1), cvCeil((ref_y - camera->img_bounds_.min_y_ + margin) * inv_cell_height));
     if (max_cell_idx_y < 0) {
         return indices;
     }
 
+    indices.reserve(undist_keypts.size());
+
     const bool check_min_level = 0 <= min_level;
     const bool check_max_level = 0 <= max_level;
 

src/stella_vslam/data/common.h

@@ -36,15 +36,17 @@ cv::Mat convert_json_to_descriptors(const nlohmann::json& json_descriptors);
  * @param keypt_indices_in_cells
  */
 void assign_keypoints_to_grid(const camera::base* camera, const std::vector<cv::KeyPoint>& undist_keypts,
-                              std::vector<std::vector<std::vector<unsigned int>>>& keypt_indices_in_cells);
+                              std::vector<std::vector<std::vector<unsigned int>>>& keypt_indices_in_cells,
+                              unsigned int num_grid_cols, unsigned int num_grid_rows);
 
 /**
  * Assign all keypoints to cells to accelerate projection matching
  * @param camera
  * @param undist_keypts
  * @return
  */
-auto assign_keypoints_to_grid(const camera::base* camera, const std::vector<cv::KeyPoint>& undist_keypts)
+auto assign_keypoints_to_grid(const camera::base* camera, const std::vector<cv::KeyPoint>& undist_keypts,
+                              unsigned int num_grid_cols, unsigned int num_grid_rows)
     -> std::vector<std::vector<std::vector<unsigned int>>>;
 
 /**
@@ -55,11 +57,14 @@ auto assign_keypoints_to_grid(const camera::base* camera, const std::vector<cv::
  * @param cell_idx_y
  * @return
  */
-inline bool get_cell_indices(const camera::base* camera, const cv::KeyPoint& keypt, int& cell_idx_x, int& cell_idx_y) {
-    cell_idx_x = cvFloor((keypt.pt.x - camera->img_bounds_.min_x_) * camera->inv_cell_width_);
-    cell_idx_y = cvFloor((keypt.pt.y - camera->img_bounds_.min_y_) * camera->inv_cell_height_);
-    return (0 <= cell_idx_x && cell_idx_x < static_cast<int>(camera->num_grid_cols_)
-            && 0 <= cell_idx_y && cell_idx_y < static_cast<int>(camera->num_grid_rows_));
+inline bool get_cell_indices(const camera::base* camera, const cv::KeyPoint& keypt,
+                             const unsigned int num_grid_cols, const unsigned int num_grid_rows,
+                             const double inv_cell_width, const double inv_cell_height,
+                             int& cell_idx_x, int& cell_idx_y) {
+    cell_idx_x = cvFloor((keypt.pt.x - camera->img_bounds_.min_x_) * inv_cell_width);
+    cell_idx_y = cvFloor((keypt.pt.y - camera->img_bounds_.min_y_) * inv_cell_height);
+    return (0 <= cell_idx_x && cell_idx_x < static_cast<int>(num_grid_cols)
+            && 0 <= cell_idx_y && cell_idx_y < static_cast<int>(num_grid_rows));
 }
 
 /**
@@ -77,6 +82,7 @@ inline bool get_cell_indices(const camera::base* camera, const cv::KeyPoint& key
 std::vector<unsigned int> get_keypoints_in_cell(const camera::base* camera, const std::vector<cv::KeyPoint>& undist_keypts,
                                                 const std::vector<std::vector<std::vector<unsigned int>>>& keypt_indices_in_cells,
                                                 const float ref_x, const float ref_y, const float margin,
+                                                const unsigned int num_grid_cols, const unsigned int num_grid_rows,
                                                 const int min_level = -1, const int max_level = -1);
 std::vector<unsigned int> get_keypoints_in_cell(const camera::base* camera, const frame_observation& frm_obs,
                                                 const float ref_x, const float ref_y, const float margin,