From cefdae30368bfe4862e2210aede012988b60dad9 Mon Sep 17 00:00:00 2001
From: Mederic Fourmy <mederic.fourmy@gmail.com>
Date: Sat, 16 Apr 2022 11:26:19 +0200
Subject: [PATCH] Empty cells active search-based feature detection
---
CMakeLists.txt | 2 +
demos/processor_visual_odometry.yaml | 13 ++-
include/vision/processor/active_search.h | 11 ++-
.../processor/processor_visual_odometry.h | 35 ++++---
src/processor/processor_visual_odometry.cpp | 94 +++++++++++++++++--
test/processor_visual_odometry_gtest.yaml | 9 +-
6 files changed, 137 insertions(+), 27 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index ce3d84398..faede5478 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -133,6 +133,7 @@ include/vision/math/pinhole_tools.h
)
SET(HDRS_PROCESSOR
include/vision/processor/processor_visual_odometry.h
+include/vision/processor/active_search.h
)
SET(HDRS_SENSOR
include/vision/sensor/sensor_camera.h
@@ -152,6 +153,7 @@ src/landmark/landmark_point_3d.cpp
)
SET(SRCS_PROCESSOR
src/processor/processor_visual_odometry.cpp
+src/processor/active_search.cpp
)
SET(SRCS_SENSOR
src/sensor/sensor_camera.cpp
diff --git a/demos/processor_visual_odometry.yaml b/demos/processor_visual_odometry.yaml
index b586000d4..d0be856d2 100644
--- a/demos/processor_visual_odometry.yaml
+++ b/demos/processor_visual_odometry.yaml
@@ -4,7 +4,7 @@ keyframe_vote:
voting_active: true
# Trigger a new keyframe creation as well as detection of new keypoints in last frame
# when the track number goes below min_features_for_keyframe in incoming
- min_features_for_keyframe: 10
+ min_features_for_keyframe: 20
#Â Use a robust cost function
apply_loss_function: true
@@ -18,9 +18,16 @@ max_new_features: 100
fast_params:
# Threshold on the keypoint pixel intensity (in uchar [0-255])
# the higher, the more selective the detector is
- threshold_fast: 50
+ threshold_fast: 30
# Avoids getting multiple keypoints at the same place
non_max_suppresion: true
+ # number of cells used by the active search grid data structure
+ active_search_grid_nb_h: 8 # horizontal
+ active_search_grid_nb_v: 8 # vertical
+ # minimum margin of the region of interest from the edges of the image
+ active_search_margin: 10
+ # reduce the size of each region of interest by n pixels to prevent keypoints from being too close
+ active_search_separation: 10
# Lucas Kanade tracking parameters
klt_params:
@@ -30,7 +37,7 @@ klt_params:
klt_max_err: 0.3
# Keep the number of tracks below
-max_nb_tracks: 20
+max_nb_tracks: 100
# standard deviation of the pixel reprojection factor
std_pix: 1
diff --git a/include/vision/processor/active_search.h b/include/vision/processor/active_search.h
index bdfca97a0..56ce70049 100644
--- a/include/vision/processor/active_search.h
+++ b/include/vision/processor/active_search.h
@@ -236,6 +236,12 @@ class ActiveSearchGrid {
*/
void blockCell(const cv::Rect & _roi);
+ Eigen::Vector2i getGridSize() {return grid_size_;}
+
+ /**
+ * \brief Get the region of interest, reduced by a margin.
+ */
+ void cell2roi(const Eigen::Vector2i & _cell, cv::Rect& _roi);
private:
/**
@@ -259,11 +265,6 @@ class ActiveSearchGrid {
*/
bool pickEmptyCell(Eigen::Vector2i & _cell);
- /**
- * \brief Get the region of interest, reduced by a margin.
- */
- void cell2roi(const Eigen::Vector2i & _cell, cv::Rect& _roi);
-
};
inline void ActiveSearchGrid::clear()
diff --git a/include/vision/processor/processor_visual_odometry.h b/include/vision/processor/processor_visual_odometry.h
index ff9bda060..cce736b44 100644
--- a/include/vision/processor/processor_visual_odometry.h
+++ b/include/vision/processor/processor_visual_odometry.h
@@ -42,8 +42,7 @@
#include "vision/feature/feature_point_image.h"
#include "vision/landmark/landmark_hp.h"
#include "vision/factor/factor_pixel_hp.h"
-
-// #include "vision/processor/klt_tracking.hpp"
+#include "vision/processor/active_search.h"
@@ -66,6 +65,10 @@ struct ParamsProcessorVisualOdometry : public ParamsProcessorTracker
{
int threshold_fast_;
bool non_max_suppresion_;
+ unsigned int active_search_grid_nb_h_;
+ unsigned int active_search_grid_nb_v_;
+ unsigned int active_search_margin_;
+ unsigned int active_search_separation_;
};
double std_pix_;
@@ -88,6 +91,10 @@ struct ParamsProcessorVisualOdometry : public ParamsProcessorTracker
fast_params_.threshold_fast_ = _server.getParam<int>( prefix + _unique_name + "/fast_params/threshold_fast");
fast_params_.non_max_suppresion_ = _server.getParam<bool>(prefix + _unique_name + "/fast_params/non_max_suppresion");
+ fast_params_.active_search_grid_nb_h_ = _server.getParam<unsigned int>(prefix + _unique_name + "/fast_params/active_search_grid_nb_h");
+ fast_params_.active_search_grid_nb_v_ = _server.getParam<unsigned int>(prefix + _unique_name + "/fast_params/active_search_grid_nb_v");
+ fast_params_.active_search_margin_ = _server.getParam<unsigned int>(prefix + _unique_name + "/fast_params/active_search_margin");
+ fast_params_.active_search_separation_ = _server.getParam<unsigned int>(prefix + _unique_name + "/fast_params/active_search_separation");
max_nb_tracks_ = _server.getParam<unsigned int>(prefix + _unique_name + "/max_nb_tracks");
min_track_length_for_landmark_ = _server.getParam<unsigned int>(prefix + _unique_name + "/min_track_length_for_landmark");
@@ -95,15 +102,19 @@ struct ParamsProcessorVisualOdometry : public ParamsProcessorTracker
}
std::string print() const override
{
- return ParamsProcessorTracker::print() + "\n"
- + "klt_params_.tracker_width_: " + std::to_string(klt_params_.patch_width_) + "\n"
- + "klt_params_.tracker_height_: " + std::to_string(klt_params_.patch_height_) + "\n"
- + "klt_params_.klt_max_err_: " + std::to_string(klt_params_.klt_max_err_) + "\n"
- + "klt_params_.nlevels_pyramids_: " + std::to_string(klt_params_.nlevels_pyramids_) + "\n"
- + "fast_params_.threshold_fast_ : " + std::to_string(fast_params_.threshold_fast_) + "\n"
- + "fast_params_.non_max_suppresion_: " + std::to_string(fast_params_.non_max_suppresion_) + "\n"
- + "max_nb_tracks_: " + std::to_string(max_nb_tracks_) + "\n"
- + "min_track_length_for_landmark_: " + std::to_string(min_track_length_for_landmark_) + "\n";
+ return ParamsProcessorTracker::print() + "\n"
+ + "klt_params_.tracker_width_: " + std::to_string(klt_params_.patch_width_) + "\n"
+ + "klt_params_.tracker_height_: " + std::to_string(klt_params_.patch_height_) + "\n"
+ + "klt_params_.klt_max_err_: " + std::to_string(klt_params_.klt_max_err_) + "\n"
+ + "klt_params_.nlevels_pyramids_: " + std::to_string(klt_params_.nlevels_pyramids_) + "\n"
+ + "fast_params_.threshold_fast_: " + std::to_string(fast_params_.threshold_fast_) + "\n"
+ + "fast_params_.non_max_suppresion_: " + std::to_string(fast_params_.non_max_suppresion_) + "\n"
+ + "fast_params_.active_search_grid_nb_h_: " + std::to_string(fast_params_.active_search_grid_nb_h_) + "\n"
+ + "fast_params_.active_search_grid_nb_v_: " + std::to_string(fast_params_.active_search_grid_nb_v_) + "\n"
+ + "fast_params_.active_search_margin_: " + std::to_string(fast_params_.active_search_grid_nb_v_) + "\n"
+ + "fast_params_.active_search_separation_: " + std::to_string(fast_params_.active_search_grid_nb_v_) + "\n"
+ + "max_nb_tracks_: " + std::to_string(max_nb_tracks_) + "\n"
+ + "min_track_length_for_landmark_: " + std::to_string(min_track_length_for_landmark_) + "\n";
}
};
@@ -133,6 +144,8 @@ class ProcessorVisualOdometry : public ProcessorTracker
CaptureImagePtr capture_image_origin_;
SensorCameraPtr sen_cam_;
+ ActiveSearchGrid cell_grid_;
+
private:
int frame_count_;
diff --git a/src/processor/processor_visual_odometry.cpp b/src/processor/processor_visual_odometry.cpp
index 735e36cc5..c047f016a 100644
--- a/src/processor/processor_visual_odometry.cpp
+++ b/src/processor/processor_visual_odometry.cpp
@@ -54,6 +54,14 @@ void ProcessorVisualOdometry::configure(SensorBasePtr _sensor)
Kcv_ = (cv::Mat_<float>(3,3) << K(0,0), 0, K(0,2),
0, K(1,1), K(1,2),
0, 0, 1);
+
+
+ // Tessalation of the image
+ cell_grid_ = ActiveSearchGrid(sen_cam_->getImgWidth(), sen_cam_->getImgHeight(),
+ params_visual_odometry_->fast_params_.active_search_grid_nb_h_,
+ params_visual_odometry_->fast_params_.active_search_grid_nb_v_,
+ params_visual_odometry_->fast_params_.active_search_margin_,
+ params_visual_odometry_->fast_params_.active_search_separation_);
}
TracksMap ProcessorVisualOdometry::mergeTracks(TracksMap tracks_prev_curr, TracksMap tracks_curr_next){
@@ -81,13 +89,44 @@ void ProcessorVisualOdometry::preProcess()
// if first image, compute keypoints, add to capture incoming and return
if (last_ptr_ == nullptr){
- // detect FAST keypoints
- std::vector<cv::KeyPoint> kps_current;
- detector_->detect(img_incoming, kps_current);
+ // detect FAST keypoints on the whole
+
+ // std::vector<cv::KeyPoint> kps_current;
+ // detector_->detect(img_incoming, kps_current);
+
+ // We add all the detected KeyPoints to the cell, knowing that they are all empty at this point
+ for (int i=0; i < params_visual_odometry_->fast_params_.active_search_grid_nb_h_; i++){
+ for (int j=0; j < params_visual_odometry_->fast_params_.active_search_grid_nb_v_; j++){
+ cv::Rect rect_roi;
+ WOLF_INFO(i, j)
+ Eigen::Vector2i cell_index; cell_index << i,j;
+ cell_grid_.cell2roi(cell_index, rect_roi);
+
+ // some cells are not inside of the image (borders + padding), if not pass
+ bool is_inside = (rect_roi & cv::Rect(0, 0, img_incoming.cols, img_incoming.rows)) == rect_roi;
+ if (!is_inside){
+ continue;
+ }
+ cv::Mat img_roi(img_incoming, rect_roi); // no data copy -> no overhead
+ std::vector<cv::KeyPoint> kps_roi;
+ detector_->detect(img_roi, kps_roi);
+
+ if (kps_roi.size() > 0){
+ // retain only the best image in each region of interest
+ cv::KeyPointsFilter::retainBest(kps_roi, 1);
+ // Keypoints are detected in the local coordinates of the region of interest
+ // -> translate to the full image corner coordinate system
+ kps_roi.at(0).pt.x = kps_roi.at(0).pt.x + rect_roi.x;
+ kps_roi.at(0).pt.y = kps_roi.at(0).pt.y + rect_roi.y;
+ // WOLF_TRACE("kps_roi first: ", kps_roi.at(0).pt.x, " ", kps_roi.at(0).pt.y)
+ capture_image_incoming_->addKeyPoints(kps_roi);
+ }
+ }
+ }
// Select a limited number of these keypoints
- cv::KeyPointsFilter::retainBest(kps_current, params_visual_odometry_->max_new_features);
- capture_image_incoming_->addKeyPoints(kps_current);
+ // cv::KeyPointsFilter::retainBest(kps_current, params_visual_odometry_->max_new_features);
+ // capture_image_incoming_->addKeyPoints(kps_current);
// Initialize the tracks data structure with a "dummy track" where the keypoint is pointing to itself
TracksMap tracks_init;
@@ -157,6 +196,9 @@ void ProcessorVisualOdometry::preProcess()
capture_image_incoming_->setTracksPrev(tracks_last_incoming_filtered);
capture_image_incoming_->setTracksOrigin(tracks_origin_incoming);
+
+ // Add valid
+
////////////////////////////////
// if too few tracks left in incoming
// detect new KeyPoints in last and track them to incoming
@@ -167,10 +209,48 @@ void ProcessorVisualOdometry::preProcess()
WOLF_TRACE( " Too Few Tracks" );
+ // Erase all keypoints previously added to the cell grid
+ cell_grid_.renew();
+
+ // Add last Keypoints that still form valid tracks between last and incoming
+ for (auto track: tracks_last_incoming_filtered){
+ size_t last_kp_id = track.first;
+ cell_grid_.hitCell(capture_image_last_->getKeyPoints().at(last_kp_id).getCvKeyPoint());
+ }
+
+
// Detect new KeyPoints
std::vector<cv::KeyPoint> kps_last_new;
- detector_->detect(img_last, kps_last_new);
- cv::KeyPointsFilter::retainBest(kps_last_new, params_visual_odometry_->max_new_features);
+
+ // Detect in the whole image and retain N best: BAD because cannot control the repartition of KeyPoints
+ // detector_->detect(img_last, kps_last_new);
+ // cv::KeyPointsFilter::retainBest(kps_last_new, params_visual_odometry_->max_new_features);
+
+ // Instead, use the grid to detect new keypoints in empty cells
+ // We try a bunch of time to add keypoints to randomly selected empty regions of interest
+ for (int i=0; i < 50; i++){
+ cv::Rect rect_roi;
+ bool is_empty = cell_grid_.pickRoi(rect_roi);
+ WOLF_TRACE("rect_roi: ", rect_roi)
+ if (!is_empty){
+ break;
+ }
+ cv::Mat img_roi(img_incoming, rect_roi); // no data copy -> no overhead
+ std::vector<cv::KeyPoint> kps_roi;
+ detector_->detect(img_roi, kps_roi);
+ if (kps_roi.size() > 0){
+ // retain only the best image in each region of interest
+ cv::KeyPointsFilter::retainBest(kps_roi, 1);
+ // Keypoints are detected in the local coordinates of the region of interest
+ // -> translate to the full image corner coordinate system
+ kps_roi.at(0).pt.x = kps_roi.at(0).pt.x + rect_roi.x;
+ kps_roi.at(0).pt.y = kps_roi.at(0).pt.y + rect_roi.y;
+ kps_last_new.push_back(kps_roi.at(0));
+ WOLF_TRACE(kps_roi.at(0).pt.x, " ", kps_roi.at(0).pt.y)
+ }
+ }
+
+
WOLF_TRACE("Detected ", kps_last_new.size(), " new raw keypoints");
// Create a map of wolf KeyPoints to track only the new ones
diff --git a/test/processor_visual_odometry_gtest.yaml b/test/processor_visual_odometry_gtest.yaml
index b586000d4..427fa2fd8 100644
--- a/test/processor_visual_odometry_gtest.yaml
+++ b/test/processor_visual_odometry_gtest.yaml
@@ -21,7 +21,14 @@ fast_params:
threshold_fast: 50
# Avoids getting multiple keypoints at the same place
non_max_suppresion: true
-
+ # number of cells used by the active search grid data structure
+ active_search_grid_nb_h: 8 # horizontal
+ active_search_grid_nb_v: 8 # vertical
+ # minimum margin of the region of interest from the edges of the image
+ active_search_margin: 10
+ # reduce the size of each region of interest by n pixels to prevent keypoints from being too close
+ active_search_separation: 10
+
# Lucas Kanade tracking parameters
klt_params:
patch_width: 15
--
GitLab