Commit 06ef4343 authored by Federico Ciuffardi's avatar Federico Ciuffardi
Browse files

frontier and criticals update fixed

parent 62216777
......@@ -52,11 +52,11 @@ bool DisMapIC::valid(CellData cd){
// save in a map (frontier, old sources)
void DisMapIC::save_frontier_old_sources(pos frontier, bool empty_sources = false){
// if is frontier and is the first time to save on the iteration
if(is_elem((*frontiers), frontier) && !is_elem(frontiers_old_sources, frontier)){
if(is_elem((*frontiers), frontier) && !is_elem(updated_frontiers, frontier)){
if(!empty_sources){
frontiers_old_sources[frontier] = cell_data[frontier].sources;
updated_frontiers[frontier] = cell_data[frontier].sources;
}else{
frontiers_old_sources[frontier] = pos_set();
updated_frontiers[frontier] = pos_set();
}
}
}
......@@ -173,8 +173,8 @@ void DisMapIC::set_consistent_borders(vector<pos> obstructed_to_free) {
}
}
}
std::cout << "consistent borders" << endl;
print_property(inserted, m->g);
//std::cout << "consistent borders" << endl;
//print_property(inserted, m->g);
}
void DisMapIC::set_source(pos p) {
......@@ -244,8 +244,8 @@ void DisMapIC::update_distance_map(pos_set source_to_true, pos_set source_to_fal
//cout << "set_source " << p << endl;
}
//clear las iteration frontiers_old_sources
frontiers_old_sources.clear();
//clear las iteration updated_frontiers
updated_frontiers.clear();
// cout<<endl<<"# Rebuild GVD"<<endl;
......
......@@ -56,9 +56,10 @@ class DisMapIC{
pos_set voro_to_true;
//specific for critical distance map, incremental set that has all the frontiers
// these are only the FREE FRONTIERS
pos_set* frontiers;
//for updating critics, old sources for frontiers are saved, new frontiers and frontiers with new critics with same distance are also saved with empty set
boost::unordered_map<pos, pos_set> frontiers_old_sources;
boost::unordered_map<pos, pos_set> updated_frontiers;
//pos_set new_frontiers;
void save_frontier_old_sources(pos frontier, bool empty_sources);
......
......@@ -372,10 +372,51 @@ void GVDIC::update_frontiers(vector<pos> unk_to_notunk) {
//print_property(frontiers, gt);
//cout << "frontier_to_true" << endl;
//print_property(frontier_to_true, gt);
//cout << "frontier_to_false" << endl;
// cout << "frontier_to_false" << endl;
//print_property(frontier_to_false, gt);
}
void GVDIC::update_free_frontiers(vector<pos> obs_to_free, vector<pos> any_to_obs){
for(auto it = frontier_to_true.begin(); it != frontier_to_true.end(); it++){
pos p = (*it);
if(gt[p.first][p.second] == Free || gt[p.first][p.second] == Frontier){
free_frontiers.insert(p);
free_frontier_to_true.insert(p);
}
}
for(auto it = frontier_to_false.begin(); it != frontier_to_false.end(); it++){
pos p = (*it);
if(gt[p.first][p.second] == Free || gt[p.first][p.second] == Frontier){
free_frontiers.erase(p);
free_frontier_to_false.insert(p);
}
}
for(auto it = obs_to_free.begin(); it != obs_to_free.end(); it++){
pos p = (*it);
if(is_elem(frontiers, p)){
free_frontiers.insert(p);
free_frontier_to_true.insert(p);
}
}
for(auto it = any_to_obs.begin(); it != any_to_obs.end(); it++){
pos p = (*it);
if(is_elem(free_frontiers, p)){
free_frontiers.erase(p);
free_frontier_to_false.insert(p);
}
}
}
void GVDIC::set_consistent_borders(vector<pos> unk_to_free) {
pos_set inserted;
for (int i = 0; i < unk_to_free.size(); i++) {
......@@ -705,9 +746,9 @@ void GVDIC::update_possible_crit_distance_map(){
disMapIC->frontiers.erase(frontier);
//disMapIC->save_frontier_old_source(f_pos); should be already saved from last iteration??
}*/
for(auto it = frontier_to_false.begin(); it != frontier_to_false.end(); it++){
for(auto it = free_frontier_to_false.begin(); it != free_frontier_to_false.end(); it++){
pos frontier = (*it);
cout<<"frontier to false "<<frontier<<endl;
cout<<"free frontier to false "<<frontier<<endl;
pos_set criticals_to_update = disMapIC->cell_data[frontier].sources;
for(auto it_crit = criticals_to_update.begin(); it_crit != criticals_to_update.end(); it_crit++){
pos critical = (*it_crit);
......@@ -717,6 +758,13 @@ void GVDIC::update_possible_crit_distance_map(){
cout<<endl;
}
//cout << "free_frontiers" << endl;
//print_property(free_frontiers, gt);
//cout << "free_frontier_to_true" << endl;
//print_property(free_frontier_to_true, gt);
//cout << "free_frontier_to_false" << endl;
//print_property(free_frontier_to_false, gt);
//cout<<"posibles criticos "<<possible_crit_to_true<<endl;
//cout<<"ex posibles criticos "<<possible_crit_to_false<<endl;
disMapIC->update_distance_map(possible_crit_to_true,possible_crit_to_false);
......@@ -787,10 +835,12 @@ void GVDIC::update_criticals(){
}
}*/
for(auto it = disMapIC->frontiers_old_sources.begin(); it != disMapIC->frontiers_old_sources.end(); it++){
for(auto it = disMapIC->updated_frontiers.begin(); it != disMapIC->updated_frontiers.end(); it++){
pos frontier = it->first;
pos_set old_criticals = it->second;
cout<<"frontier to erase from old source "<<frontier<<endl;
if(!old_criticals.empty()){
cout<<"frontier to erase from old source "<<frontier<<endl;
}
// delete frotier from old critic
for(auto it_old_crit = old_criticals.begin(); it_old_crit != old_criticals.end(); it_old_crit++){
pos critical = (*it_old_crit);
......@@ -801,7 +851,9 @@ void GVDIC::update_criticals(){
// add or update (change new criticals name)
pos_set new_criticals = disMapIC->cell_data[frontier].sources;
cout<<"frontier to add to new source "<<frontier<<endl;
if(!new_criticals.empty()){
cout << "frontier to add to new source " << frontier << endl;
}
for(auto it_new_crit = new_criticals.begin(); it_new_crit != new_criticals.end(); it_new_crit++){
pos critical = (*it_new_crit);
cout<<"criticals to update "<<critical<<" ";
......@@ -856,6 +908,9 @@ boost::tuple<criticals_info, GVD> GVDIC::get_points_of_interest(grid gt) {
frontier_to_true.clear();
frontier_to_false.clear();
free_frontier_to_true.clear();
free_frontier_to_false.clear();
possible_crit_to_true.clear();
possible_crit_to_false.clear();
......@@ -869,6 +924,7 @@ boost::tuple<criticals_info, GVD> GVDIC::get_points_of_interest(grid gt) {
boost::tie(obs_to_free, any_to_obs, unk_to_free, unk_to_notunk) = get_changes(gt);
update_frontiers(unk_to_notunk);
update_free_frontiers(obs_to_free, any_to_obs);
update_obstacle_distance_map(obs_to_free, any_to_obs, unk_to_free);
update_gvd();
......@@ -890,7 +946,7 @@ boost::tuple<criticals_info, GVD> GVDIC::get_points_of_interest(grid gt) {
GVDIC::GVDIC(){
m = new Map();
disMapIC = new DisMapIC(m,{Occupied,Unknown}, &frontiers);
disMapIC = new DisMapIC(m,{Occupied,Unknown}, &free_frontiers);
}
GVDIC::~GVDIC(){
......
......@@ -53,6 +53,7 @@ class GVDIC {
void remove_obstacle(pos p);
void set_consistent_borders(vector<pos>);
void update_frontiers(vector<pos>);
void update_free_frontiers(vector<pos> obs_to_free, vector<pos> any_to_obs);
void update_possible_crit_distance_map();
void set_voro(bool value, pos n);
......@@ -105,6 +106,10 @@ class GVDIC {
pos_set frontier_to_false;
pos_set frontier_to_true;
pos_set free_frontiers;
pos_set free_frontier_to_true;
pos_set free_frontier_to_false;
pos_set possible_crit_to_false;
pos_set possible_crit_to_true;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment