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

Optimize orb_extractor #582

Merged
merged 1 commit into from
Apr 7, 2024
Merged

Optimize orb_extractor #582

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
82 changes: 49 additions & 33 deletions src/stella_vslam/feature/orb_extractor.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -77,6 +77,16 @@ void orb_extractor::extract(const cv::_InputArray& in_image, const cv::_InputArr
keypts.reserve(num_keypts);

unsigned int offset = 0;
std::vector<unsigned int> offsets;
offsets.push_back(0);
for (unsigned int level = 0; level < orb_params_->num_levels_ - 1; ++level) {
offset += all_keypts.at(level).size();
offsets.push_back(offset);
}

#ifdef USE_OPENMP
#pragma omp parallel for schedule(dynamic)
#endif
for (unsigned int level = 0; level < orb_params_->num_levels_; ++level) {
auto& keypts_at_level = all_keypts.at(level);
const auto num_keypts_at_level = keypts_at_level.size();
Expand All @@ -85,16 +95,26 @@ void orb_extractor::extract(const cv::_InputArray& in_image, const cv::_InputArr
continue;
}

cv::Mat blurred_image = image_pyramid_.at(level).clone();
cv::GaussianBlur(blurred_image, blurred_image, cv::Size(7, 7), 2, 2, cv::BORDER_REFLECT_101);
cv::Mat blurred_image;
cv::GaussianBlur(image_pyramid_.at(level), blurred_image, cv::Size(7, 7), 2, 2, cv::BORDER_REFLECT_101);

cv::Mat descriptors_at_level = descriptors.rowRange(offset, offset + num_keypts_at_level);
compute_orb_descriptors(blurred_image, keypts_at_level, descriptors_at_level);
cv::Mat descriptors_at_level = descriptors.rowRange(offsets[level], offsets[level] + num_keypts_at_level);
descriptors_at_level = cv::Mat::zeros(num_keypts_at_level, 32, CV_8UC1);

offset += num_keypts_at_level;
// To enable parallelization, set the environment variable OMP_MAX_ACTIVE_LEVELS to 2.
#ifdef USE_OPENMP
#pragma omp parallel for schedule(static)
#endif
for (unsigned int i = 0; i < keypts_at_level.size(); ++i) {
compute_orb_descriptor(keypts_at_level[i], blurred_image, descriptors_at_level.ptr(i));
}

correct_keypoint_scale(keypts_at_level, level);
}

// Collect keypoints for every scale
for (unsigned int level = 0; level < orb_params_->num_levels_; ++level) {
auto& keypts_at_level = all_keypts.at(level);
keypts.insert(keypts.end(), keypts_at_level.begin(), keypts_at_level.end());
}
}
Expand Down Expand Up @@ -137,7 +157,7 @@ void orb_extractor::compute_fast_keypoints(std::vector<std::vector<cv::KeyPoint>
constexpr unsigned int cell_size = 64;

#ifdef USE_OPENMP
#pragma omp parallel for
#pragma omp parallel for schedule(dynamic)
#endif
for (int64_t level = 0; level < orb_params_->num_levels_; ++level) {
const float scale_factor = orb_params_->scale_factors_.at(level);
Expand All @@ -150,13 +170,15 @@ void orb_extractor::compute_fast_keypoints(std::vector<std::vector<cv::KeyPoint>
const unsigned int width = max_border_x - min_border_x;
const unsigned int height = max_border_y - min_border_y;

const unsigned int num_cols = std::ceil(width / cell_size) + 1;
const unsigned int num_rows = std::ceil(height / cell_size) + 1;
const unsigned int num_cols = width / cell_size + 1;
const unsigned int num_rows = height / cell_size + 1;

std::vector<cv::KeyPoint> keypts_to_distribute;
keypts_to_distribute.reserve(500);

// To enable parallelization, set the environment variable OMP_MAX_ACTIVE_LEVELS to 2.
#ifdef USE_OPENMP
#pragma omp parallel for
#pragma omp parallel for schedule(static)
#endif
for (int64_t i = 0; i < num_rows; ++i) {
const unsigned int min_y = min_border_y + i * cell_size;
Expand All @@ -168,9 +190,6 @@ void orb_extractor::compute_fast_keypoints(std::vector<std::vector<cv::KeyPoint>
max_y = max_border_y;
}

#ifdef USE_OPENMP
#pragma omp parallel for
#endif
for (int64_t j = 0; j < num_cols; ++j) {
const unsigned int min_x = min_border_x + j * cell_size;
if (max_border_x - overlap <= min_x) {
Expand Down Expand Up @@ -203,20 +222,28 @@ void orb_extractor::compute_fast_keypoints(std::vector<std::vector<cv::KeyPoint>
continue;
}

// Collect keypoints for every scale
#ifdef USE_OPENMP
#pragma omp critical
#endif
{
for (auto& keypt : keypts_in_cell) {
keypt.pt.x += j * cell_size;
keypt.pt.y += i * cell_size;
for (auto& keypt : keypts_in_cell) {
keypt.pt.x += j * cell_size;
keypt.pt.y += i * cell_size;
}

if (!mask.empty()) {
std::vector<cv::KeyPoint> keypts_in_cell_masked;
for (auto&& keypt : keypts_in_cell) {
// Check if the keypoint is in the mask
if (!mask.empty() && is_in_mask(min_border_y + keypt.pt.y, min_border_x + keypt.pt.x, scale_factor)) {
if (is_in_mask(min_border_y + keypt.pt.y, min_border_x + keypt.pt.x, scale_factor)) {
continue;
}
keypts_to_distribute.push_back(keypt);
keypts_in_cell_masked.push_back(std::move(keypt));
}
keypts_in_cell = std::move(keypts_in_cell_masked);
}

#ifdef USE_OPENMP
#pragma omp critical
#endif
{
keypts_to_distribute.insert(keypts_to_distribute.end(), keypts_in_cell.begin(), keypts_in_cell.end());
}
}
}
Expand All @@ -238,10 +265,7 @@ void orb_extractor::compute_fast_keypoints(std::vector<std::vector<cv::KeyPoint>
keypt.octave = level;
keypt.size = scaled_patch_size;
}
}

// Compute orientations
for (unsigned int level = 0; level < orb_params_->num_levels_; ++level) {
compute_orientation(image_pyramid_.at(level), all_keypts.at(level));
}
}
Expand Down Expand Up @@ -308,14 +332,6 @@ float orb_extractor::ic_angle(const cv::Mat& image, const cv::Point2f& point) co
return orb_impl_.ic_angle(image, point);
}

void orb_extractor::compute_orb_descriptors(const cv::Mat& image, const std::vector<cv::KeyPoint>& keypts, cv::Mat& descriptors) const {
descriptors = cv::Mat::zeros(keypts.size(), 32, CV_8UC1);

for (unsigned int i = 0; i < keypts.size(); ++i) {
compute_orb_descriptor(keypts.at(i), image, descriptors.ptr(i));
}
}

void orb_extractor::compute_orb_descriptor(const cv::KeyPoint& keypt, const cv::Mat& image, uchar* desc) const {
orb_impl_.compute_orb_descriptor(keypt, image, desc);
}
Expand Down
3 changes: 0 additions & 3 deletions src/stella_vslam/feature/orb_extractor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,9 +63,6 @@ class orb_extractor {
//! Compute the gradient direction of pixel intensity in a circle around the point
float ic_angle(const cv::Mat& image, const cv::Point2f& point) const;

//! Compute orb descriptors for all keypoint
void compute_orb_descriptors(const cv::Mat& image, const std::vector<cv::KeyPoint>& keypts, cv::Mat& descriptors) const;

//! Compute orb descriptor of a keypoint
void compute_orb_descriptor(const cv::KeyPoint& keypt, const cv::Mat& image, uchar* desc) const;

Expand Down
Loading