Skip to content
Snippets Groups Projects
Commit 7319a532 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Put grid params out of fast params

parent 9d38726b
No related branches found
No related tags found
2 merge requests!36After cmake and const refactor,!28Resolve "Building a new visual odometry system"
......@@ -65,15 +65,20 @@ 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_;
};
struct GridParams
{
unsigned int nbr_cells_h_;
unsigned int nbr_cells_v_;
unsigned int margin_;
unsigned int separation_;
};
double std_pix_;
KltParams klt_params_;
FastParams fast_params_;
GridParams grid_params_;
unsigned int max_nb_tracks_;
unsigned int min_track_length_for_landmark_;
......@@ -91,10 +96,11 @@ 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");
grid_params_.nbr_cells_h_ = _server.getParam<unsigned int>(prefix + _unique_name + "/grid_params/nbr_cells_h");
grid_params_.nbr_cells_v_ = _server.getParam<unsigned int>(prefix + _unique_name + "/grid_params/nbr_cells_v");
grid_params_.margin_ = _server.getParam<unsigned int>(prefix + _unique_name + "/grid_params/margin");
grid_params_.separation_ = _server.getParam<unsigned int>(prefix + _unique_name + "/grid_params/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");
......@@ -109,10 +115,10 @@ struct ParamsProcessorVisualOdometry : public ParamsProcessorTracker
+ "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"
+ "grid_params_.nbr_cells_h_: " + std::to_string(grid_params_.nbr_cells_h_) + "\n"
+ "grid_params_.nbr_cells_v_: " + std::to_string(grid_params_.nbr_cells_v_) + "\n"
+ "grid_params_.margin_: " + std::to_string(grid_params_.margin_) + "\n"
+ "grid_params_.separation_: " + std::to_string(grid_params_.separation_) + "\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";
}
......
......@@ -58,10 +58,10 @@ void ProcessorVisualOdometry::configure(SensorBasePtr _sensor)
// 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_);
params_visual_odometry_->grid_params_.nbr_cells_h_,
params_visual_odometry_->grid_params_.nbr_cells_v_,
params_visual_odometry_->grid_params_.margin_,
params_visual_odometry_->grid_params_.separation_);
}
TracksMap ProcessorVisualOdometry::mergeTracks(TracksMap tracks_prev_curr, TracksMap tracks_curr_next){
......@@ -96,8 +96,8 @@ void ProcessorVisualOdometry::preProcess()
// 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++){
for (int i=0; i < params_visual_odometry_->grid_params_.nbr_cells_h_; i++){
for (int j=0; j < params_visual_odometry_->grid_params_.nbr_cells_v_; j++){
cv::Rect rect_roi;
WOLF_INFO(i, j)
Eigen::Vector2i cell_index; cell_index << i,j;
......@@ -279,13 +279,13 @@ void ProcessorVisualOdometry::preProcess()
// Only keep tracks until it reaches a max nb of tracks
// TODO: the strategy for keeping the best new tracks is dumb
// -> should be improved for a better spatial repartition
unsigned int count_new_tracks = 0;
// unsigned int count_new_tracks = 0;
for (auto & track: tracks_last_incoming_new){
if ((n_tracks_origin + count_new_tracks) >= params_visual_odometry_->max_nb_tracks_){
break;
}
// if ((n_tracks_origin + count_new_tracks) >= params_visual_odometry_->max_nb_tracks_){
// break;
// }
tracks_last_incoming_filtered[track.first] = track.second;
count_new_tracks++;
// count_new_tracks++;
}
WOLF_INFO("New total : ", n_tracks_origin, " + ", mwkps_incoming_new.size(), " = ", tracks_last_incoming_filtered.size(), " tracks");
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment