From 9bade2ebaf2ef17dfedade68d3ede45955383472 Mon Sep 17 00:00:00 2001 From: Lev Denisov <98268579+denisovGIT@users.noreply.github.com> Date: Fri, 20 Oct 2023 23:17:02 +0300 Subject: [PATCH 1/2] fix: optimization "DepthImage" algorithm stage (#47) * fix: Fixed syntax errors and the use of objects in OpenMP * fix: add std::move * fix: The initialization of the structure was made to the constructor and parallel sections were added * fix: the name of the private field of the class has been changed * fix: fixed access to class fields --- cpp/deplex/include/deplex/utils/depth_image.h | 5 +++ cpp/deplex/src/deplex/utils/depth_image.cpp | 32 ++++++++++++++----- 2 files changed, 29 insertions(+), 8 deletions(-) diff --git a/cpp/deplex/include/deplex/utils/depth_image.h b/cpp/deplex/include/deplex/utils/depth_image.h index 9066c74..17d436d 100644 --- a/cpp/deplex/include/deplex/utils/depth_image.h +++ b/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 image_; int32_t width_; int32_t height_; + + Eigen::VectorXf column_indices_; + Eigen::VectorXf row_indices_; }; } // namespace utils } // namespace deplex \ No newline at end of file diff --git a/cpp/deplex/src/deplex/utils/depth_image.cpp b/cpp/deplex/src/deplex/utils/depth_image.cpp index 20bab31..a6ec278 100644 --- a/cpp/deplex/src/deplex/utils/depth_image.cpp +++ b/cpp/deplex/src/deplex/utils/depth_image.cpp @@ -34,6 +34,18 @@ 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(); +} + +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_; } @@ -41,24 +53,28 @@ 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::Matrix 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::type t_image; pcd_points.col(2) = Eigen::Map>(image_.get(), width_ * height_).cast(); - 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 \ No newline at end of file From 0fd2cfa11116734696da41b232f15bda7419e253 Mon Sep 17 00:00:00 2001 From: Lev Denisov <98268579+denisovGIT@users.noreply.github.com> Date: Sun, 22 Oct 2023 18:49:50 +0300 Subject: [PATCH 2/2] fix: optimization "createPlaneSegments" algorithm stage (#45) * fix: vector replacement * fix: replaced and removed the getNeighbours function * fix: removed debag libraries * fix: editing under clang-format * fix: the names of the class fields have been changed and links have been removed * fix: fixed the conflict * fix: delete include --- cpp/deplex/src/deplex/cell_grid.cpp | 12 ----- cpp/deplex/src/deplex/cell_grid.h | 7 --- cpp/deplex/src/deplex/plane_extractor.cpp | 65 +++++++++++++---------- 3 files changed, 38 insertions(+), 46 deletions(-) diff --git a/cpp/deplex/src/deplex/cell_grid.cpp b/cpp/deplex/src/deplex/cell_grid.cpp index 1190ada..b4ba670 100644 --- a/cpp/deplex/src/deplex/cell_grid.cpp +++ b/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 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, 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 c2fe995..c546635 100644 --- a/cpp/deplex/src/deplex/plane_extractor.cpp +++ b/cpp/deplex/src/deplex/plane_extractor.cpp @@ -18,6 +18,7 @@ #include #include #include + #if defined(DEBUG_DEPLEX) || defined(BENCHMARK_LOGGING) #include #include @@ -73,6 +74,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. @@ -132,8 +134,8 @@ 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, - CellGrid const& cell_grid) const; + std::vector growSeed(Eigen::Index seed_id, std::vector 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 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 +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()); - // 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 +346,21 @@ std::vector PlaneExtractor::Impl::createPlaneSegments(CellGrid cons return plane_segments; } -std::vector PlaneExtractor::Impl::growSeed(Eigen::Index seed_id, std::vector const& unassigned, - CellGrid const& cell_grid) const { +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 +369,26 @@ 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)) { + 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(neighbour)); } } } - return activation_map; + + return cells_to_merge; } std::vector PlaneExtractor::Impl::findMergedLabels(std::vector* plane_segments) {