Commit 62216777 authored by Romina Julieta Parada Venossa's avatar Romina Julieta Parada Venossa
Browse files

critical recognition with frontiers

parent b55e9900
......@@ -49,10 +49,17 @@ bool DisMapIC::valid(CellData cd){
return false;
}
/*
void DisMapIC::save_frontier_old_sources(pos frontier){
frontiers_old_sources[frontier] = cell_data[frontier].sources;
}*/
// 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(!empty_sources){
frontiers_old_sources[frontier] = cell_data[frontier].sources;
}else{
frontiers_old_sources[frontier] = pos_set();
}
}
}
void DisMapIC::process_lower(pos p) {
CellData& p_cd = cell_data[p];
......@@ -66,6 +73,10 @@ void DisMapIC::process_lower(pos p) {
// cout<<n<<" d "<< d <<" dist "<< n_cd.dist <<endl;
float d = dist(n,p) + p_cd.dist;
if (d < n_cd.dist) {
//specific for critic detection
//if is a frontier then save old sources
save_frontier_old_sources(n);
// cout << "lowered: " <<n << endl;
n_cd.dist = d;
// cout<<n<<"updated dist to "<< n_cd.dist <<endl;
......@@ -75,7 +86,13 @@ void DisMapIC::process_lower(pos p) {
n_cd.sources.insert(p_cd.sources.begin(), p_cd.sources.end());
n_cd.is_cleared = false;
open.push(make_pair(d, n));
} else if (d == n_cd.dist) {
//specific for critic detection
//if is a frontier then add it to the map so it will update critis
//should use a different collection(different meaning)
save_frontier_old_sources(n, false);
n_cd.parents.insert(p);
n_cd.sources.insert(p_cd.sources.begin(), p_cd.sources.end());
}
......@@ -124,6 +141,10 @@ void DisMapIC::process_raise(pos p) {
}
if (min_d < MAXFLOAT) {
//specific for critic detection
//if is a frontier then save old sources
save_frontier_old_sources(p);
p_cd.dist = min_d;
p_cd.parents.clear();
p_cd.parents.insert(p_parents.begin(), p_parents.end());
......@@ -223,6 +244,9 @@ 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();
// cout<<endl<<"# Rebuild GVD"<<endl;
raise_and_lower();
......
......@@ -57,10 +57,11 @@ class DisMapIC{
//specific for critical distance map, incremental set that has all the frontiers
pos_set* frontiers;
//boost::unordered_map<pos, pos_set> frontiers_old_sources;
//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;
//pos_set new_frontiers;
//void save_frontier_old_sources(pos frontier);
void save_frontier_old_sources(pos frontier, bool empty_sources);
void update_distance_map(pos_set source_to_true, pos_set source_to_false);
......
......@@ -154,7 +154,7 @@ void GVDIC::check_voro(pos s, pos n) {
CellDataO& n_cd = cell_data_o[n];
if (n_cd.obsts.size() > 1) {
set_voro(true, n);
set_voro(true, n);
}
if (s_cd.obsts.size() > 1) {
set_voro(true, s);
......@@ -705,25 +705,139 @@ 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++){
pos frontier = (*it);
cout<<"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);
cout<<"criticals to update "<<critical<<" ";
erase_frontier(critical, frontier);
}
cout<<endl;
}
//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);
}
/*void GVDIC::update_crit(){
void GVDIC::update_criticals_mind_f(){
for(auto it = criticals_to_update_mind_f.begin(); it != criticals_to_update_mind_f.end(); it++){
pos critical = (*it);
if(criticals[critical].to_update){
criticals[critical].mind_f.second = MAXFLOAT;
for(auto it_f = criticals[critical].frontiers.begin(); it_f != criticals[critical].frontiers.end(); it_f++){
pos frontier = (*it_f);
float f_dist = disMapIC->cell_data[frontier].dist;
if(f_dist < criticals[critical].mind_f.second){
criticals[critical].mind_f.first = frontier;
criticals[critical].mind_f.second = f_dist;
}
}
criticals[critical].to_update = false;
}
}
criticals_to_update_mind_f.clear();
}
void GVDIC::erase_frontier(pos critical, pos frontier){
criticals[critical].erase_frontier(frontier, disMapIC->cell_data[frontier].dist);
if(criticals[critical].frontiers.size() == 0){
criticals.erase(critical);
}else{
if(criticals[critical].to_update){
criticals_to_update_mind_f.insert(critical);
}
}
}
void GVDIC::add_frontier(pos critical, pos frontier){
if(!is_elem(criticals, critical)){
criticals[critical] = critical_info2();
criticals[critical].mind_f = std::pair<pos, float>(pos(),MAXFLOAT);
criticals[critical].to_update = false;
}
criticals[critical].add_frontier(frontier, disMapIC->cell_data[frontier].dist);
if(criticals[critical].to_update){
criticals_to_update_mind_f.insert(critical);
}
}
void GVDIC::update_criticals(){
//old crits
//for each frontier_to_false and frontier changed remove frontier from old sources
//delete from crit frontiers
// if no longer crit then
//delete from crit
//paso 1 (agregar descricpion del trello)
/*
for(auto it = frontier_to_false.begin(); it != frontier_to_false.end(); it++){
pos frontier = (*it);
cout<<"frontier to eliminate "<<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);
cout<<"critical to update "<<critical<<endl;
erase_frontier(critical, frontier);
}
}*/
for(auto it = disMapIC->frontiers_old_sources.begin(); it != disMapIC->frontiers_old_sources.end(); it++){
pos frontier = it->first;
pos_set old_criticals = it->second;
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);
cout<<"criticals to update "<<critical<<" ";
erase_frontier(critical, frontier);
}
cout<<endl;
// 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;
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<<" ";
add_frontier(critical, frontier);
/*if(!is_elem(criticals, critical)){
criticals[critical] = critical_info2();
criticals[critical].mind_f = pair(pos(),MAXFLOAT);
}
criticals[critical].frontiers.insert(frontier);
float frontier_dist = disMapIC->cell_data[frontier].dist;
if(criticals[critical].mind_f > frontier_dist){
criticals[critical].mind_f = frontier_dist;
}*/
}
cout<<endl;
}
update_criticals_mind_f();
for(auto it = criticals.begin(); it != criticals.end(); it++){
cout<<(*it).first<<" fronteras: ";
for(auto it_f = (*it).second.frontiers.begin();it_f != (*it).second.frontiers.end(); it_f++){
cout<<(*it_f)<<" ";
}
cout<<endl;
}
//delete from old_source frontier the ones in frontier_to_false
//new crits
//for each forntier that changed
//add crit or update crit
}*/
}
//////////
// MAIN
......@@ -766,7 +880,7 @@ boost::tuple<criticals_info, GVD> GVDIC::get_points_of_interest(grid gt) {
// print_property(possible_crit,gt);
update_possible_crit_distance_map();
//update_crit();
update_criticals();
// post increment
old_gt = gt;
......
......@@ -73,6 +73,7 @@ class GVDIC {
bool is_local_min(pos p);
pos_set get_to_update_min(pos_set to_change);
void update_possible_crit(pos_set to_add, pos_set to_remove);
void update_criticals();
bool has_neighbour_of_deg(pos p, int deg);
......@@ -108,5 +109,12 @@ class GVDIC {
pos_set possible_crit_to_true;
pos_set possible_crit;
criticals_info2 criticals;
pos_set criticals_to_update_mind_f;
void update_criticals_mind_f();
void erase_frontier(pos critical, pos frontier);
void add_frontier(pos critical, pos frontier);
};
#endif
......@@ -15,4 +15,37 @@ struct critical_info {
};
typedef boost::unordered_map<pos, critical_info> criticals_info;
struct critical_info2 {
pair<pos,float> mind_f; // min distance to frontier
pos_set frontiers;
bool to_update;
void add_frontier(pos p, float d){
if(p == mind_f.first){
if (d <= mind_f.second){
mind_f.second = d;
}else{
//update min
to_update = true;
}
}else{
if(d < mind_f.second){
mind_f.second = d;
to_update = false;
}
frontiers.insert(p);
}
}
void erase_frontier(pos p, float d){
if(p == mind_f.first){
to_update = true;
}
frontiers.erase(p);
}
};
typedef boost::unordered_map<pos, critical_info2> criticals_info2;
#endif
\ No newline at end of file
......@@ -528,7 +528,103 @@ vector<grid> test_12 = {
},
};
vector<vector<grid>> tests = {test_1, test_2, test_3, test_4, test_5, test_6, test_7, test_8,test_9,test_10,test_11,test_12};
vector<grid> test_13 = {
{
{Unknown, Unknown, Unknown, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
{Unknown, Unknown, Unknown, Free , Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Unknown, Unknown, Unknown, Free , Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Unknown, Unknown, Unknown, Free , Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Unknown, Unknown, Unknown, Free , Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Unknown, Unknown, Unknown, Free , Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Unknown, Unknown, Unknown, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
},
{
{Unknown, Unknown, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
{Unknown, Unknown, Free , Free , Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Unknown, Free , Free , Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Unknown, Free , Free , Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Unknown, Unknown, Free , Free , Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Unknown, Free , Free , Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Unknown, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
},
{
{Unknown, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
{Unknown, Free , Free , Occupied, Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Free , Free , Occupied, Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Free , Free , Free , Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Unknown, Free , Free , Occupied, Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Free , Free , Occupied, Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
},
{
{Unknown, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
{Unknown, Free , Free , Occupied, Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Free , Free , Occupied, Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Free , Free , Free , Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Free , Free , Occupied, Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Free , Free , Occupied, Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
},
};
vector<grid> test_14 = {
{
{Unknown, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Unknown},
{Unknown, Free , Free , Free , Free , Free , Free , Free ,Free , Unknown},
{Unknown, Free , Free , Free , Free , Free , Free , Free ,Free , Unknown},
{Unknown, Free , Free , Free , Free , Free , Free , Free ,Free , Unknown},
{Unknown, Free , Free , Free , Free , Free , Free , Free ,Free , Unknown},
{Unknown, Free , Free , Free , Free , Free , Free , Free ,Free , Unknown},
{Unknown, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Unknown},
},
{
{Unknown, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Unknown},
{Unknown, Free , Free , Free , Free , Occupied, Free , Free ,Free , Unknown},
{Unknown, Free , Free , Free , Free , Occupied, Free , Free ,Free , Unknown},
{Unknown, Free , Free , Free , Free , Free , Free , Free ,Free , Unknown},
{Unknown, Free , Free , Free , Free , Occupied, Free , Free ,Free , Unknown},
{Unknown, Free , Free , Free , Free , Occupied, Free , Free ,Free , Unknown},
{Unknown, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Unknown},
},
{
{Unknown, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Unknown},
{Unknown, Free , Free , Free , Free , Occupied, Free , Free ,Free , Unknown},
{Unknown, Free , Free , Free , Free , Occupied, Free , Free ,Free , Unknown},
{Unknown, Occupied, Occupied, Free , Free , Free , Free , Free ,Free , Unknown},
{Unknown, Free , Free , Free , Free , Occupied, Free , Free ,Free , Unknown},
{Unknown, Free , Free , Free , Free , Occupied, Free , Free ,Free , Unknown},
{Unknown, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Unknown},
},
{
{Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
{Free , Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Free , Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Free , Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Free , Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Free , Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
},
{
{Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
{Occupied, Occupied, Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Occupied, Occupied, Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Occupied, Occupied, Free , Free , Free , Free , Free , Free ,Free , Free },
{Occupied, Occupied, Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Occupied, Occupied, Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
},
{
{Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
{Free , Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Free , Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Free , Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Free , Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Free , Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
},
};
vector<vector<grid>> tests = {test_1, test_2, test_3, test_4, test_5, test_6, test_7, test_8,test_9,test_10,test_11,test_12, test_13, test_14};
#define RESET "\033[0m"
#define BLACK "\033[30m" /* Black */
......@@ -640,7 +736,11 @@ int main(int argc, char** argv) {
} else if (igvdc.gvd.is_uncollapsed(p)) {
string colour = CYAN;
if(is_elem(igvdc.possible_crit, p)){
colour = GREEN;
if(is_elem(igvdc.criticals, p)){
colour = YELLOW;
}else{
colour = GREEN;
}
}
cout << "|" << colorize(" O ", colour);
} else if (igvdc.gvd.is_collapsed(p)) {
......
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