From d6af59bee6c15b692aa63db0489495c85a364a7b Mon Sep 17 00:00:00 2001 From: Lev Denisov Date: Sat, 23 Sep 2023 19:38:29 +0300 Subject: [PATCH 1/7] fix: vector replacement --- cpp/deplex/src/deplex/plane_extractor.cpp | 50 +++++++++++------------ 1 file changed, 25 insertions(+), 25 deletions(-) diff --git a/cpp/deplex/src/deplex/plane_extractor.cpp b/cpp/deplex/src/deplex/plane_extractor.cpp index c6610ee..3e4a5b9 100644 --- a/cpp/deplex/src/deplex/plane_extractor.cpp +++ b/cpp/deplex/src/deplex/plane_extractor.cpp @@ -132,7 +132,7 @@ class PlaneExtractor::Impl { * @param cell_grid Cell Grid. * @returns Vector of activated cells after growSeed call. */ - std::vector growSeed(Eigen::Index seed_id, std::vector const& unassigned, + std::vector growSeed(Eigen::Index seed_id, std::vector const& unassigned, CellGrid const& cell_grid) const; /** @@ -303,20 +303,17 @@ std::vector PlaneExtractor::Impl::createPlaneSegments(CellGrid cons } // 3. Grow seed CellSegment plane_candidate(cell_grid[seed_id]); - std::vector activation_map = growSeed(seed_id, unassigned_mask, cell_grid); + std::vector 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(i)); - unassigned_mask[i] = false; - --remaining_planar_cells; - } + for (auto &v : cells_to_merge) { + plane_candidate += cell_grid[v]; + hist.removePoint(static_cast(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,16 +323,8 @@ std::vector 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(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(v) / nr_horizontal_cells_)[static_cast(v) % nr_horizontal_cells_] = nr_curr_planes; } } } @@ -343,17 +332,22 @@ std::vector PlaneExtractor::Impl::createPlaneSegments(CellGrid cons return plane_segments; } -std::vector PlaneExtractor::Impl::growSeed(Eigen::Index seed_id, std::vector const& unassigned, +std::vector PlaneExtractor::Impl::growSeed(Eigen::Index seed_id, std::vector const& unassigned, CellGrid const& cell_grid) const { + std::vector activation_map(unassigned.size(), false); + std::vector cells_to_merge; if (!unassigned[seed_id]) { - return activation_map; + return cells_to_merge; } + cells_to_merge.reserve(unassigned.size()); + std::queue 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 +356,28 @@ std::vector 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)) { + std::vector neighbours = cell_grid.getNeighbours(current_seed); + + for (auto neighbour : neighbours) { 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(neighbour)); } } } - return activation_map; + + return cells_to_merge; } std::vector PlaneExtractor::Impl::findMergedLabels(std::vector* plane_segments) { From b4d8eaf5f9eb6b355c3fc448a407a3a69980239f Mon Sep 17 00:00:00 2001 From: Lev Denisov Date: Sun, 24 Sep 2023 03:14:51 +0300 Subject: [PATCH 2/7] fix: replaced and removed the getNeighbours function --- cpp/deplex/src/deplex/cell_grid.cpp | 17 +++-------------- cpp/deplex/src/deplex/cell_grid.h | 7 ------- cpp/deplex/src/deplex/plane_extractor.cpp | 19 ++++++++++++++++--- 3 files changed, 19 insertions(+), 24 deletions(-) diff --git a/cpp/deplex/src/deplex/cell_grid.cpp b/cpp/deplex/src/deplex/cell_grid.cpp index 3b3843b..2c0a716 100644 --- a/cpp/deplex/src/deplex/cell_grid.cpp +++ b/cpp/deplex/src/deplex/cell_grid.cpp @@ -15,6 +15,7 @@ */ #include "cell_grid.h" +#include #include namespace deplex { @@ -33,7 +34,7 @@ CellGrid::CellGrid(Eigen::Matrix cons Eigen::Map> cell_points(cell_continuous_points.data(), cell_width_ * cell_height_, 3); -#pragma omp parallel for default(none) shared(cell_continuous_points, config, cell_points) +#pragma omp parallel for shared(cell_continuous_points, config) firstprivate(cell_points) for (Eigen::Index cell_id = 0; cell_id < number_horizontal_cells_ * number_vertical_cells_; ++cell_id) { Eigen::Index offset = cell_id * cell_height_ * cell_width_ * 3; new (&cell_points) decltype(cell_points)(cell_continuous_points.data() + offset, cell_width_ * cell_height_, 3); @@ -64,24 +65,12 @@ CellSegment const& CellGrid::operator[](size_t cell_id) const { return cell_grid std::vector const& CellGrid::getPlanarMask() const { return planar_mask_; } -std::vector CellGrid::getNeighbours(size_t cell_id) const { - std::vector 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 const& unorganized_data, Eigen::Matrix* organized_pcd) { int32_t image_width = number_horizontal_cells_ * cell_width_; -#pragma omp parallel for default(none) shared(cell_width, cell_height, organized_pcd, unorganized_data) +#pragma omp parallel for shared(cell_width_, cell_height_, organized_pcd, unorganized_data, image_width) for (Eigen::Index cell_id = 0; cell_id < number_horizontal_cells_ * number_vertical_cells_; ++cell_id) { Eigen::Index outer_cell_stride = cell_width_ * cell_height_ * cell_id; for (Eigen::Index i = 0; i < cell_height_; ++i) { diff --git a/cpp/deplex/src/deplex/cell_grid.h b/cpp/deplex/src/deplex/cell_grid.h index 13c02d9..6c7672e 100644 --- a/cpp/deplex/src/deplex/cell_grid.h +++ b/cpp/deplex/src/deplex/cell_grid.h @@ -72,13 +72,6 @@ class CellGrid { */ std::vector const& getPlanarMask() const; - /** - * Get cell 2D-neighbours indices - * - * @returns Vector of neighbours indices (Maximum 4 neighbours) - */ - std::vector getNeighbours(size_t cell_id) const; - /** * Number of total cells * diff --git a/cpp/deplex/src/deplex/plane_extractor.cpp b/cpp/deplex/src/deplex/plane_extractor.cpp index 3e4a5b9..12f3385 100644 --- a/cpp/deplex/src/deplex/plane_extractor.cpp +++ b/cpp/deplex/src/deplex/plane_extractor.cpp @@ -18,6 +18,9 @@ #include #include #include +#include +#include + #if defined(DEBUG_DEPLEX) || defined(BENCHMARK_LOGGING) #include #include @@ -73,6 +76,7 @@ class PlaneExtractor::Impl { int32_t image_height_; int32_t image_width_; Eigen::MatrixXi labels_map_; + std::vector> neighbours; /** * Initialize histogram from planar cells of cell grid. @@ -160,6 +164,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; @@ -356,9 +371,7 @@ std::vector PlaneExtractor::Impl::growSeed(Eigen::Index seed_id, std::ve double d_current = cell_grid[current_seed].getStat().getD(); Eigen::Vector3f normal_current = cell_grid[current_seed].getStat().getNormal(); - std::vector neighbours = cell_grid.getNeighbours(current_seed); - - for (auto neighbour : neighbours) { + for (size_t neighbour : neighbours[current_seed]) { if (!unassigned[neighbour] || activation_map[neighbour]) { continue; } From 21bdd9fbf2448da428b00a4dc8f806a8a8925250 Mon Sep 17 00:00:00 2001 From: Lev Denisov Date: Sun, 24 Sep 2023 03:20:59 +0300 Subject: [PATCH 3/7] fix: removed debag libraries --- cpp/deplex/src/deplex/plane_extractor.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/cpp/deplex/src/deplex/plane_extractor.cpp b/cpp/deplex/src/deplex/plane_extractor.cpp index 12f3385..b11f6b5 100644 --- a/cpp/deplex/src/deplex/plane_extractor.cpp +++ b/cpp/deplex/src/deplex/plane_extractor.cpp @@ -18,8 +18,6 @@ #include #include #include -#include -#include #if defined(DEBUG_DEPLEX) || defined(BENCHMARK_LOGGING) #include From fa01bf6552c487e58e1967542d493b788cd6fa4b Mon Sep 17 00:00:00 2001 From: Lev Denisov Date: Sun, 24 Sep 2023 03:44:38 +0300 Subject: [PATCH 4/7] fix: editing under clang-format --- cpp/deplex/src/deplex/plane_extractor.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/cpp/deplex/src/deplex/plane_extractor.cpp b/cpp/deplex/src/deplex/plane_extractor.cpp index b11f6b5..6fc7519 100644 --- a/cpp/deplex/src/deplex/plane_extractor.cpp +++ b/cpp/deplex/src/deplex/plane_extractor.cpp @@ -135,7 +135,7 @@ class PlaneExtractor::Impl { * @returns Vector of activated cells after growSeed call. */ std::vector growSeed(Eigen::Index seed_id, std::vector const& unassigned, - CellGrid const& cell_grid) const; + CellGrid const& cell_grid) const; /** * Get vector of potentially mergeable cell components. @@ -319,7 +319,7 @@ std::vector PlaneExtractor::Impl::createPlaneSegments(CellGrid cons std::vector cells_to_merge = growSeed(seed_id, unassigned_mask, cell_grid); // 4. Merge activated cells & remove from hist - for (auto &v : cells_to_merge) { + for (auto& v : cells_to_merge) { plane_candidate += cell_grid[v]; hist.removePoint(static_cast(v)); unassigned_mask[v] = false; @@ -336,8 +336,9 @@ std::vector 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(plane_segments.size()); - for (auto &v : cells_to_merge) { - labels_map_.row(static_cast(v) / nr_horizontal_cells_)[static_cast(v) % nr_horizontal_cells_] = nr_curr_planes; + for (auto& v : cells_to_merge) { + labels_map_.row(static_cast(v) / nr_horizontal_cells_)[static_cast(v) % nr_horizontal_cells_] = + nr_curr_planes; } } } @@ -346,8 +347,7 @@ std::vector PlaneExtractor::Impl::createPlaneSegments(CellGrid cons } std::vector PlaneExtractor::Impl::growSeed(Eigen::Index seed_id, std::vector const& unassigned, - CellGrid const& cell_grid) const { - + CellGrid const& cell_grid) const { std::vector activation_map(unassigned.size(), false); std::vector cells_to_merge; From 2defd8a3aeea90f8220bb0f362ca402ab0adf46b Mon Sep 17 00:00:00 2001 From: Lev Denisov Date: Fri, 20 Oct 2023 19:14:00 +0300 Subject: [PATCH 5/7] fix: the names of the class fields have been changed and links have been removed --- cpp/deplex/src/deplex/plane_extractor.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/cpp/deplex/src/deplex/plane_extractor.cpp b/cpp/deplex/src/deplex/plane_extractor.cpp index 6fc7519..c6461c0 100644 --- a/cpp/deplex/src/deplex/plane_extractor.cpp +++ b/cpp/deplex/src/deplex/plane_extractor.cpp @@ -74,7 +74,7 @@ class PlaneExtractor::Impl { int32_t image_height_; int32_t image_width_; Eigen::MatrixXi labels_map_; - std::vector> neighbours; + std::vector> neighbours_; /** * Initialize histogram from planar cells of cell grid. @@ -163,15 +163,15 @@ PlaneExtractor::Impl::Impl(int32_t image_height, int32_t image_width, config::Co "). patchSize has to be positive."); } - neighbours.resize(nr_horizontal_cells_ * nr_vertical_cells_); + 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); + 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); } } @@ -319,7 +319,7 @@ std::vector PlaneExtractor::Impl::createPlaneSegments(CellGrid cons std::vector cells_to_merge = growSeed(seed_id, unassigned_mask, cell_grid); // 4. Merge activated cells & remove from hist - for (auto& v : cells_to_merge) { + for (auto v : cells_to_merge) { plane_candidate += cell_grid[v]; hist.removePoint(static_cast(v)); unassigned_mask[v] = false; @@ -336,7 +336,7 @@ std::vector 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(plane_segments.size()); - for (auto& v : cells_to_merge) { + for (auto v : cells_to_merge) { labels_map_.row(static_cast(v) / nr_horizontal_cells_)[static_cast(v) % nr_horizontal_cells_] = nr_curr_planes; } @@ -369,7 +369,7 @@ std::vector PlaneExtractor::Impl::growSeed(Eigen::Index seed_id, std::ve double d_current = cell_grid[current_seed].getStat().getD(); Eigen::Vector3f normal_current = cell_grid[current_seed].getStat().getNormal(); - for (size_t neighbour : neighbours[current_seed]) { + for (size_t neighbour : neighbours_[current_seed]) { if (!unassigned[neighbour] || activation_map[neighbour]) { continue; } From 3d6529d37277002d72d2b7aa6b1f86bafe0116dc Mon Sep 17 00:00:00 2001 From: Lev Denisov Date: Fri, 20 Oct 2023 19:27:26 +0300 Subject: [PATCH 6/7] fix: fixed the conflict --- cpp/deplex/src/deplex/cell_grid.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cpp/deplex/src/deplex/cell_grid.cpp b/cpp/deplex/src/deplex/cell_grid.cpp index 2c0a716..3c70b95 100644 --- a/cpp/deplex/src/deplex/cell_grid.cpp +++ b/cpp/deplex/src/deplex/cell_grid.cpp @@ -34,7 +34,7 @@ CellGrid::CellGrid(Eigen::Matrix cons Eigen::Map> cell_points(cell_continuous_points.data(), cell_width_ * cell_height_, 3); -#pragma omp parallel for shared(cell_continuous_points, config) firstprivate(cell_points) +#pragma omp parallel for default(none) shared(cell_continuous_points, config) firstprivate(cell_points) for (Eigen::Index cell_id = 0; cell_id < number_horizontal_cells_ * number_vertical_cells_; ++cell_id) { Eigen::Index offset = cell_id * cell_height_ * cell_width_ * 3; new (&cell_points) decltype(cell_points)(cell_continuous_points.data() + offset, cell_width_ * cell_height_, 3); @@ -70,7 +70,7 @@ size_t CellGrid::size() const { return planar_mask_.size(); } void CellGrid::cellContinuousOrganize(Eigen::Matrix const& unorganized_data, Eigen::Matrix* organized_pcd) { int32_t image_width = number_horizontal_cells_ * cell_width_; -#pragma omp parallel for shared(cell_width_, cell_height_, organized_pcd, unorganized_data, image_width) +#pragma omp parallel for default(none) shared(cell_width_, cell_height_, organized_pcd, unorganized_data, image_width) for (Eigen::Index cell_id = 0; cell_id < number_horizontal_cells_ * number_vertical_cells_; ++cell_id) { Eigen::Index outer_cell_stride = cell_width_ * cell_height_ * cell_id; for (Eigen::Index i = 0; i < cell_height_; ++i) { From 6bd2e3f59c27bf8654a86443ba8dc46a49fe08c1 Mon Sep 17 00:00:00 2001 From: Lev Denisov Date: Fri, 20 Oct 2023 23:27:11 +0300 Subject: [PATCH 7/7] fix: delete include --- cpp/deplex/src/deplex/cell_grid.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/cpp/deplex/src/deplex/cell_grid.cpp b/cpp/deplex/src/deplex/cell_grid.cpp index 3c70b95..4acdb86 100644 --- a/cpp/deplex/src/deplex/cell_grid.cpp +++ b/cpp/deplex/src/deplex/cell_grid.cpp @@ -15,7 +15,6 @@ */ #include "cell_grid.h" -#include #include namespace deplex {