Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix: optimization "createPlaneSegments" algorithm stage #45

Merged
merged 7 commits into from
Oct 22, 2023
Merged

fix: optimization "createPlaneSegments" algorithm stage #45

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
d6af59b
fix: vector replacement
LevDenisov Sep 23, 2023
b4d8eaf
fix: replaced and removed the getNeighbours function
LevDenisov Sep 24, 2023
21bdd9f
fix: removed debag libraries
LevDenisov Sep 24, 2023
fa01bf6
fix: editing under clang-format
LevDenisov Sep 24, 2023
2defd8a
fix: the names of the class fields have been changed and links have b…
LevDenisov Oct 20, 2023
3d6529d
fix: fixed the conflict
LevDenisov Oct 20, 2023
6bd2e3f
fix: delete include
LevDenisov Oct 20, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 3 additions & 14 deletions cpp/deplex/src/deplex/cell_grid.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
*/
#include "cell_grid.h"

#include <iostream>
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Remove iostream

#include <utility>

namespace deplex {
Expand All @@ -33,7 +34,7 @@ CellGrid::CellGrid(Eigen::Matrix<float, Eigen::Dynamic, 3, Eigen::RowMajor> cons

Eigen::Map<const Eigen::Matrix<float, Eigen::Dynamic, 3, Eigen::RowMajor>> 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 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);
Expand Down Expand Up @@ -64,24 +65,12 @@ 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,
Eigen::Matrix<float, Eigen::Dynamic, 3, Eigen::RowMajor>* 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 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) {
Expand Down
7 changes: 0 additions & 7 deletions cpp/deplex/src/deplex/cell_grid.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
*
Expand Down
65 changes: 38 additions & 27 deletions cpp/deplex/src/deplex/plane_extractor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include <algorithm>
#include <numeric>
#include <queue>

#if defined(DEBUG_DEPLEX) || defined(BENCHMARK_LOGGING)
#include <fstream>
#include <iostream>
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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.
Expand All @@ -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;
Expand Down Expand Up @@ -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;
}

Expand All @@ -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();
Expand All @@ -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) {
Expand Down
Loading