Merge branch 'prime-slam:stable' into benchmark-artifact

cpp/deplex/include/deplex/utils/depth_image.h

@@ -44,10 +44,15 @@ class DepthImage {
    */
   Eigen::MatrixX3f toPointCloud(Eigen::Matrix3f const& intrinsics) const;
 
+  void reset(std::string const& image_path);
+
  private:
   std::unique_ptr<unsigned short> image_;
   int32_t width_;
   int32_t height_;
+
+  Eigen::VectorXf column_indices_;
+  Eigen::VectorXf row_indices_;
 };
 }  // namespace utils
 }  // namespace deplex

cpp/deplex/src/deplex/cell_grid.cpp

@@ -64,18 +64,6 @@ CellSegment const& CellGrid::operator[](size_t cell_id) const { return cell_grid
 
 std::vector<bool> const& CellGrid::getPlanarMask() const { return planar_mask_; }
 
-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) 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) neighbours.push_back(cell_id - 1);
-  if (y + 1 < number_horizontal_cells_) neighbours.push_back(cell_id + 1);
-
-  return neighbours;
-}
-
 size_t CellGrid::size() const { return planar_mask_.size(); }
 
 void CellGrid::cellContinuousOrganize(Eigen::Matrix<float, Eigen::Dynamic, 3, Eigen::RowMajor> const& unorganized_data,

cpp/deplex/src/deplex/cell_grid.h

@@ -72,13 +72,6 @@ class CellGrid {
    */
   std::vector<bool> const& getPlanarMask() const;
 
-  /**
-   * Get cell 2D-neighbours indices
-   *
-   * @returns Vector of neighbours indices (Maximum 4 neighbours)
-   */
-  std::vector<size_t> getNeighbours(size_t cell_id) const;
-
   /**
    * Number of total cells
    *

cpp/deplex/src/deplex/plane_extractor.cpp

@@ -18,6 +18,7 @@
 #include <algorithm>
 #include <numeric>
 #include <queue>
+
 #if defined(DEBUG_DEPLEX) || defined(BENCHMARK_LOGGING)
 #include <fstream>
 #include <iostream>
@@ -73,6 +74,7 @@ class PlaneExtractor::Impl {
   int32_t image_height_;
   int32_t image_width_;
   Eigen::MatrixXi labels_map_;
+  std::vector<std::vector<Eigen::Index>> neighbours_;
 
   /**
    * Initialize histogram from planar cells of cell grid.
@@ -132,8 +134,8 @@ class PlaneExtractor::Impl {
    * @param cell_grid Cell Grid.
    * @returns Vector of activated cells after growSeed call.
    */
-  std::vector<bool> growSeed(Eigen::Index seed_id, std::vector<bool> const& unassigned,
-                             CellGrid const& cell_grid) const;
+  std::vector<size_t> growSeed(Eigen::Index seed_id, std::vector<bool> const& unassigned,
+                               CellGrid const& cell_grid) const;
 
   /**
    * Get vector of potentially mergeable cell components.
@@ -160,6 +162,17 @@ PlaneExtractor::Impl::Impl(int32_t image_height, int32_t image_width, config::Co
     throw std::runtime_error("Error! Invalid config parameter: patchSize(" + std::to_string(config.patch_size) +
                              "). patchSize has to be positive.");
   }
+
+  neighbours_.resize(nr_horizontal_cells_ * nr_vertical_cells_);
+
+  for (Eigen::Index i = 0; i < nr_horizontal_cells_ * nr_vertical_cells_; ++i) {
+    size_t x = i / nr_horizontal_cells_;
+    size_t y = i % nr_horizontal_cells_;
+    if (x >= 1) neighbours_[i].push_back(i - nr_horizontal_cells_);
+    if (x + 1 < nr_vertical_cells_) neighbours_[i].push_back(i + nr_horizontal_cells_);
+    if (y >= 1) neighbours_[i].push_back(i - 1);
+    if (y + 1 < nr_horizontal_cells_) neighbours_[i].push_back(i + 1);
+  }
 }
 
 PlaneExtractor::~PlaneExtractor() = default;
@@ -303,20 +316,17 @@ std::vector<CellSegment> PlaneExtractor::Impl::createPlaneSegments(CellGrid cons
     }
     // 3. Grow seed
     CellSegment plane_candidate(cell_grid[seed_id]);
-    std::vector<bool> activation_map = growSeed(seed_id, unassigned_mask, cell_grid);
+    std::vector<size_t> cells_to_merge = growSeed(seed_id, unassigned_mask, cell_grid);
 
     // 4. Merge activated cells & remove from hist
-    for (size_t i = 0; i < activation_map.size(); ++i) {
-      if (activation_map[i]) {
-        plane_candidate += cell_grid[i];
-        hist.removePoint(static_cast<int32_t>(i));
-        unassigned_mask[i] = false;
-        --remaining_planar_cells;
-      }
+    for (auto v : cells_to_merge) {
+      plane_candidate += cell_grid[v];
+      hist.removePoint(static_cast<int32_t>(v));
+      unassigned_mask[v] = false;
+      --remaining_planar_cells;
     }
-    size_t nr_cells_activated = std::count(activation_map.begin(), activation_map.end(), true);
 
-    if (nr_cells_activated < config_.min_region_growing_cells_activated) {
+    if (cells_to_merge.size() < config_.min_region_growing_cells_activated) {
       continue;
     }
 
@@ -326,34 +336,31 @@ std::vector<CellSegment> PlaneExtractor::Impl::createPlaneSegments(CellGrid cons
     if (plane_candidate.getStat().getScore() > config_.min_region_planarity_score) {
       plane_segments.push_back(plane_candidate);
       auto nr_curr_planes = static_cast<int32_t>(plane_segments.size());
-      // Mark cells
-      // TODO: Effective assigning by mask?
-      int stacked_cell_id = 0;
-      for (int32_t row_id = 0; row_id < nr_vertical_cells_; ++row_id) {
-        for (int32_t col_id = 0; col_id < nr_horizontal_cells_; ++col_id) {
-          if (activation_map[stacked_cell_id]) {
-            labels_map_.row(row_id)[col_id] = nr_curr_planes;
-          }
-          ++stacked_cell_id;
-        }
+      for (auto v : cells_to_merge) {
+        labels_map_.row(static_cast<long>(v) / nr_horizontal_cells_)[static_cast<long>(v) % nr_horizontal_cells_] =
+            nr_curr_planes;
       }
     }
   }
 
   return plane_segments;
 }
 
-std::vector<bool> PlaneExtractor::Impl::growSeed(Eigen::Index seed_id, std::vector<bool> const& unassigned,
-                                                 CellGrid const& cell_grid) const {
+std::vector<size_t> PlaneExtractor::Impl::growSeed(Eigen::Index seed_id, std::vector<bool> const& unassigned,
+                                                   CellGrid const& cell_grid) const {
   std::vector<bool> activation_map(unassigned.size(), false);
+  std::vector<size_t> cells_to_merge;
 
   if (!unassigned[seed_id]) {
-    return activation_map;
+    return cells_to_merge;
   }
 
+  cells_to_merge.reserve(unassigned.size());
+
   std::queue<Eigen::Index> seed_queue;
   seed_queue.push(seed_id);
   activation_map[seed_id] = true;
+  cells_to_merge.push_back(seed_id);
 
   while (!seed_queue.empty()) {
     Eigen::Index current_seed = seed_queue.front();
@@ -362,22 +369,26 @@ std::vector<bool> PlaneExtractor::Impl::growSeed(Eigen::Index seed_id, std::vect
     double d_current = cell_grid[current_seed].getStat().getD();
     Eigen::Vector3f normal_current = cell_grid[current_seed].getStat().getNormal();
 
-    for (auto neighbour : cell_grid.getNeighbours(current_seed)) {
+    for (size_t neighbour : neighbours_[current_seed]) {
       if (!unassigned[neighbour] || activation_map[neighbour]) {
         continue;
       }
+
       Eigen::Vector3f normal_neighbour = cell_grid[neighbour].getStat().getNormal();
       Eigen::Vector3f mean_neighbour = cell_grid[neighbour].getStat().getMean();
 
       double cos_angle = normal_current.dot(normal_neighbour);
       double merge_dist = pow(normal_current.dot(mean_neighbour) + d_current, 2);
+
       if (cos_angle >= config_.min_cos_angle_merge && merge_dist <= cell_grid[neighbour].getMergeTolerance()) {
         activation_map[neighbour] = true;
+        cells_to_merge.push_back(neighbour);
         seed_queue.push(static_cast<Eigen::Index>(neighbour));
       }
     }
   }
-  return activation_map;
+
+  return cells_to_merge;
 }
 
 std::vector<int32_t> PlaneExtractor::Impl::findMergedLabels(std::vector<CellSegment>* plane_segments) {

cpp/deplex/src/deplex/utils/depth_image.cpp

@@ -34,31 +34,47 @@ DepthImage::DepthImage(std::string const& image_path) : width_(0), height_(0) {
     throw std::runtime_error("Error: Couldn't read image " + image_path);
   }
   image_.reset(data_ptr);
+
+  column_indices_ = Eigen::VectorXf::LinSpaced(width_, 0.0, width_ - 1).replicate(height_, 1);
+  row_indices_ = Eigen::VectorXf::LinSpaced(height_, 0.0, height_ - 1).replicate(1, width_).reshaped<Eigen::RowMajor>();
+}
+
+void DepthImage::reset(std::string const& image_path) {
+  int32_t actual_channels = 0;
+  auto data_ptr = stbi_load_16(image_path.c_str(), &width_, &height_, &actual_channels, STBI_grey);
+  if (data_ptr == nullptr) {
+    throw std::runtime_error("Error: Couldn't read image " + image_path);
+  }
+  image_.reset(data_ptr);
 }
 
 int32_t DepthImage::getWidth() const { return width_; }
 
 int32_t DepthImage::getHeight() const { return height_; }
 
 Eigen::MatrixX3f DepthImage::toPointCloud(Eigen::Matrix3f const& intrinsics) const {
-  Eigen::VectorXf column_indices = Eigen::VectorXf::LinSpaced(width_, 0.0, width_ - 1).replicate(height_, 1);
-  Eigen::VectorXf row_indices =
-      Eigen::VectorXf::LinSpaced(height_, 0.0, height_ - 1).replicate(1, width_).reshaped<Eigen::RowMajor>();
-
   Eigen::Matrix<float, Eigen::Dynamic, 3, Eigen::RowMajor> pcd_points(width_ * height_, 3);
 
   float fx = intrinsics.row(0)[0];
-  float fy = intrinsics.row(1)[1];
   float cx = intrinsics.row(0)[2];
+  float fy = intrinsics.row(1)[1];
   float cy = intrinsics.row(1)[2];
 
   typedef std::remove_reference<decltype(*image_)>::type t_image;
   pcd_points.col(2) = Eigen::Map<Eigen::Vector<t_image, Eigen::Dynamic>>(image_.get(), width_ * height_).cast<float>();
-  pcd_points.col(0) = (column_indices.array() - cx) * pcd_points.col(2).array() / fx;
-  pcd_points.col(1) = (row_indices.array() - cy) * pcd_points.col(2).array() / fy;
+
+#pragma omp parallel default(none) shared(pcd_points, column_indices, row_indices, cx, cy, fx, fy)
+  {
+#pragma omp sections
+    {
+#pragma omp section
+      { pcd_points.col(0) = (column_indices_.array() - cx) * pcd_points.col(2).array() / fx; }
+#pragma omp section
+      { pcd_points.col(1) = (row_indices_.array() - cy) * pcd_points.col(2).array() / fy; }
+    }
+  }
 
   return pcd_points;
 }
-
 }  // namespace utils
 }  // namespace deplex