Algorithm refactor

cpp/deplex/src/deplex/cell_grid.cpp

@@ -22,25 +22,40 @@ CellGrid::CellGrid(Eigen::MatrixXf const& points, config::Config const& config,
       cell_height_(config.getInt("patchSize")),
       number_horizontal_cells_(number_horizontal_cells),
       number_vertical_cells_(number_vertical_cells),
-      cell_grid_(number_vertical_cells),
-      planar_mask_(number_horizontal_cells_ * number_vertical_cells_) {
+      parent_(number_vertical_cells * number_horizontal_cells),
+      component_size_(number_vertical_cells * number_horizontal_cells, 1),
+      planar_mask_(number_vertical_cells * number_horizontal_cells) {
+  cell_grid_.reserve(planar_mask_.size());
   int32_t stacked_cell_id = 0;
   for (Eigen::Index cell_row = 0; cell_row < number_vertical_cells; ++cell_row) {
-    cell_grid_[cell_row].reserve(number_horizontal_cells);
     for (Eigen::Index cell_col = 0; cell_col < number_horizontal_cells; ++cell_col) {
       Eigen::Index offset = stacked_cell_id * cell_width_ * cell_height_;
       Eigen::MatrixXf cell_points = points.block(offset, 0, cell_width_ * cell_height_, 3);
-      cell_grid_[cell_row].emplace_back(cell_points, config);
-      planar_mask_[stacked_cell_id] = cell_grid_[cell_row][cell_col].isPlanar();
+      parent_[stacked_cell_id] = stacked_cell_id;
+      cell_grid_.emplace_back(cell_points, config);
+      planar_mask_[stacked_cell_id] = cell_grid_[stacked_cell_id].isPlanar();
       ++stacked_cell_id;
     }
   }
 }
 
-CellSegment const& CellGrid::getByCoordinates(size_t x, size_t y) const { return cell_grid_[x][y]; }
+CellSegment const& CellGrid::operator[](size_t cell_id) const { return cell_grid_[cell_id]; }
 
-CellSegment const& CellGrid::operator[](size_t cell_id) const {
-  return cell_grid_[cell_id / number_horizontal_cells_][cell_id % number_horizontal_cells_];
+size_t CellGrid::findLabel(size_t cell_id) {
+  return (parent_[cell_id] == cell_id) ? cell_id : parent_[cell_id] = findLabel(parent_[cell_id]);
+}
+
+void CellGrid::uniteLabels(size_t a, size_t b) {
+  a = findLabel(a);
+  b = findLabel(b);
+  if (component_size_[a] > component_size_[b]) std::swap(a, b);
+  component_size_[b] += component_size_[a];
+  parent_[a] = b;
+}
+
+void CellGrid::updateCell(size_t cell_id, CellSegment new_cell) {
+  planar_mask_[cell_id] = new_cell.isPlanar();
+  cell_grid_[cell_id] = std::move(new_cell);
 }
 
 std::vector<bool> const& CellGrid::getPlanarMask() const { return planar_mask_; }
@@ -49,9 +64,9 @@ std::vector<size_t> CellGrid::getNeighbours(size_t cell_id) const {
   std::vector<size_t> neighbours;
   size_t x = cell_id / number_horizontal_cells_;
   size_t y = cell_id % number_horizontal_cells_;
-  if (x - 1 >= 0) neighbours.push_back(cell_id - number_horizontal_cells_);
+  if (x >= 1) neighbours.push_back(cell_id - number_horizontal_cells_);
   if (x + 1 < number_vertical_cells_) neighbours.push_back(cell_id + number_horizontal_cells_);
-  if (y - 1 >= 0) neighbours.push_back(cell_id - 1);
+  if (y >= 1) neighbours.push_back(cell_id - 1);
   if (y + 1 < number_horizontal_cells_) neighbours.push_back(cell_id + 1);
 
   return neighbours;

cpp/deplex/src/deplex/cell_grid.h

@@ -25,7 +25,11 @@ class CellGrid {
   CellGrid(Eigen::MatrixXf const& points, config::Config const& config, int32_t number_horizontal_cells,
            int32_t number_vertical_cells);
 
-  CellSegment const& getByCoordinates(size_t x, size_t y) const;
+  size_t findLabel(size_t v);
+
+  void uniteLabels(size_t a, size_t b);
+
+  void updateCell(size_t cell_id, CellSegment new_cell);
 
   CellSegment const& operator[](size_t cell_id) const;
 
@@ -40,7 +44,9 @@ class CellGrid {
   int32_t cell_height_;
   int32_t number_horizontal_cells_;
   int32_t number_vertical_cells_;
-  std::vector<std::vector<CellSegment>> cell_grid_;
+  std::vector<size_t> parent_;
+  std::vector<int> component_size_;
+  std::vector<CellSegment> cell_grid_;
   std::vector<bool> planar_mask_;
 };
 }  // namespace deplex

cpp/deplex/src/deplex/cell_segment.cpp

@@ -16,7 +16,12 @@
 #include "cell_segment.h"
 
 namespace deplex {
-CellSegment::CellSegment(Eigen::MatrixXf const& cell_points, config::Config const& config) : is_planar_(false) {
+CellSegment::CellSegment() : stats_(), is_planar_(false), merge_tolerance_(), min_merge_cos_(), max_merge_dist_() {}
+
+CellSegment::CellSegment(Eigen::MatrixXf const& cell_points, config::Config const& config)
+    : is_planar_(false),
+      min_merge_cos_(config.getFloat("minCosAngleForMerge")),
+      max_merge_dist_(config.getFloat("maxMergeDist")) {
   size_t valid_pts_threshold = cell_points.size() / config.getInt("minPtsPerCell");
   int32_t cell_width = config.getInt("patchSize");
   int32_t cell_height = config.getInt("patchSize");
@@ -38,6 +43,13 @@ CellSegment& CellSegment::operator+=(CellSegment const& other) {
   return *this;
 }
 
+bool CellSegment::areNeighbours3D(CellSegment const& other) const {
+  if (!this->is_planar_ || !other.is_planar_) return false;
+  auto cos_angle = this->getStat().getNormal().dot(other.getStat().getNormal());
+  auto distance = pow(this->getStat().getNormal().dot(other.getStat().getMean()) + this->getStat().getD(), 2);
+  return cos_angle >= min_merge_cos_ && distance <= max_merge_dist_;
+}
+
 CellSegmentStat const& CellSegment::getStat() const { return stats_; };
 
 bool CellSegment::isPlanar() const { return is_planar_; }

cpp/deplex/src/deplex/cell_segment.h

@@ -23,6 +23,8 @@
 namespace deplex {
 class CellSegment {
  public:
+  CellSegment();
+
   CellSegment(Eigen::MatrixXf const& cell_points, config::Config const& config);
 
   CellSegment& operator+=(CellSegment const& other);
@@ -33,12 +35,16 @@ class CellSegment {
 
   bool isPlanar() const;
 
+  bool areNeighbours3D(CellSegment const& other) const;
+
   float getMergeTolerance() const;
 
  private:
   CellSegmentStat stats_;
   bool is_planar_;
   float merge_tolerance_;
+  float min_merge_cos_;
+  float max_merge_dist_;
 
   bool hasValidPoints(Eigen::MatrixXf const& cell_points, size_t valid_pts_threshold) const;