obst_condition

src/lib/GVD/GVDIC.cpp

 #include "GVDIC.h"
 #include <cmath>
+#include <vector>
 #include "pos.h"
 #include "utils.h"
 
@@ -25,27 +26,89 @@ boost::tuple<float, vector<pos>> closest_obstacles(pos p, pos_set obsts) {
   return boost::make_tuple(d, closest_obsts);
 }
 
-pos_set obst_condition_change; // contanis all the cells that had 2 or more obstacles but now have 1 or less
+pos_set obst_cond_to_false; // contanis all the cells that had 2 or more obstacles but now have 1 or less
+pos_set updated_obst_size;
+
+
+// wrapers to set (or replace) and to insert (or union) obstacles that checks if obst_cond_to_false must be modified
+void GVDIC::set_obsts(CellDataO &cd, pos_set new_obsts, pos p){
+  cd.obsts = new_obsts;
+  int new_size = cd.obsts.size();
+  if ( cd.old_obsts_size > 1 && new_size > 1){
+    obst_cond_to_false.erase(p);
+  }
+  if(cd.old_obsts_size !=new_size){
+    updated_obst_size.insert(p);
+  }else {
+    updated_obst_size.erase(p);
+  }
+}
+
+void GVDIC::set_obsts(CellDataO &cd, vector<pos> new_obsts, pos p){
+  cd.obsts.clear();
+  cd.obsts.insert(new_obsts.begin(),new_obsts.end());
+  int new_size = cd.obsts.size();
+  if ( cd.old_obsts_size > 1 && new_size > 1){
+    obst_cond_to_false.erase(p);
+  }
+  if(cd.old_obsts_size !=new_size){
+    updated_obst_size.insert(p);
+  }else {
+    updated_obst_size.erase(p);
+  }
+}
+
+void GVDIC::insert_obsts(CellDataO &cd,pos_set new_obsts,pos p){
+  cd.obsts.insert(new_obsts.begin(),new_obsts.end());
+  int new_size = cd.obsts.size();
+  if ( cd.old_obsts_size > 1 && new_size > 1){
+    obst_cond_to_false.erase(p);
+  }
+  if(cd.old_obsts_size !=new_size){
+    updated_obst_size.insert(p);
+  }else {
+    updated_obst_size.erase(p);
+  }
+}
+
+void GVDIC::insert_obsts(CellDataO &cd,vector<pos> new_obsts,pos p){
+  cd.obsts.insert(new_obsts.begin(),new_obsts.end());
+  int new_size = cd.obsts.size();
+  if ( cd.old_obsts_size > 1 && new_size > 1){
+    obst_cond_to_false.erase(p);
+  }
+  if(cd.old_obsts_size !=new_size){
+    updated_obst_size.insert(p);
+  }else {
+    updated_obst_size.erase(p);
+  }
+  
+}
 
 // recheck obstacles of n removing the invalid ones O(1) as you can only
 // have up to 8 equidistant obstacles in a 2D grid
-bool GVDIC::is_occ(pos_set& obsts, pos p) {
-  int old_size = obsts.size();
-  for (auto it = obsts.begin(); it != obsts.end();) {
+bool GVDIC::is_occ(CellDataO& cd, pos p) {
+  for (auto it = cd.obsts.begin(); it != cd.obsts.end();) {
     pos obst = (*it);
     if (gt[obst.first][obst.second] != Occupied) {
-      it = obsts.erase(it);
+      it = cd.obsts.erase(it);
     } else {
       ++it;
     }
   }
-  int new_size = obsts.size();
-  /* cout<<p<<"("<<old_size<<","<<new_size<<")"<<endl; */
-  if ( old_size > 1 && new_size == 1){
+  int new_size = cd.obsts.size();
+  // cout << p << "(" << cd.old_obsts_size << "," << new_size << ")" << endl;
+  // cd.dist > 0 if and only if p is not an obstacle 
+  if ( cd.old_obsts_size > 1 && new_size <= 1 && cd.dist > 0){
     /* cout<<"insert "<<p<<endl; */
-    obst_condition_change.insert(p);
+    obst_cond_to_false.insert(p);
   }      
-  return obsts.size() > 0;
+  if(cd.old_obsts_size !=new_size){
+    updated_obst_size.insert(p);
+  }else {
+    updated_obst_size.erase(p);
+  }
+  return cd.obsts.size() > 0;
 }
 
 // CellDataO
@@ -129,7 +192,7 @@ void GVDIC::check_voro(pos s, pos n) {
   pos obst_s = *s_cd.obsts.begin();
   auto adj_n = adj(gt, obst_n);
 
-  if (is_occ(n_cd.obsts,n) && (obst_n != obst_s) && !is_elem(adj_n, obst_s)) {
+  if (is_occ(n_cd,n) && (obst_n != obst_s) && !is_elem(adj_n, obst_s)) {
     if ((dist(s, obst_n) <= dist(n, obst_s)) && s_cd.dist > 0) {
       set_voro(true, s);
     }
@@ -160,16 +223,15 @@ void GVDIC::process_lower(pos s) {
         // cout<<n<<"updated dist to "<< n_cd.dist <<endl;
         n_cd.parents.clear();
         n_cd.parents.insert(s);
-        n_cd.obsts.clear();
-        n_cd.obsts.insert(s_closest_obsts.begin(), s_closest_obsts.end());
+        set_obsts(n_cd,s_closest_obsts,n);
         n_cd.is_cleared = false;
         open.push(make_pair(d, n));
       } else {
         if (d == n_cd.dist) {
-          n_cd.obsts.insert(s_closest_obsts.begin(), s_closest_obsts.end());
+          insert_obsts(n_cd,s_closest_obsts,n);
           n_cd.parents.insert(s);
         }
-        if (is_occ(n_cd.obsts,n)) {
+        if (is_occ(n_cd,n)) {
           check_voro(s, n);
           // cout << "check_voro" << n << endl;
         }
@@ -192,7 +254,7 @@ void GVDIC::process_raise(pos s) {
     if (!n_cd.is_cleared && !n_cd.to_raise) {
       // if n does not have any valid obstacle then its distance is invalid
       // it and should propagate the process raise
-      if (!is_occ(n_cd.obsts,n)) {
+      if (!is_occ(n_cd,n)) {
         // cout << "raised: " <<n << endl;
         open.push(make_pair(n_cd.dist, n));
         n_cd.clear_cell();
@@ -223,8 +285,8 @@ void GVDIC::process_raise(pos s) {
     s_cd.dist = min_d;
     s_cd.parents.clear();
     s_cd.parents.insert(s_parents.begin(), s_parents.end());
-    s_cd.obsts.clear();
-    s_cd.obsts.insert(s_closest_obsts.begin(), s_closest_obsts.end());
+
+    set_obsts(s_cd,s_closest_obsts,s);
     s_cd.is_cleared = false;
     open.push(make_pair(s_cd.dist, s));
   }
@@ -260,13 +322,13 @@ void GVDIC::update_obstacle_distance_map(vector<pos> obs_to_free,
     if (s_cd.to_raise) {
       // cout << "Process RAISE    "<<s << endl;
       process_raise(s);
-    } else if (is_occ(s_cd.obsts,s)) {
+    } else if (is_occ(s_cd,s)) {
       // cout << "Process LOWER    "<<s << endl;
       set_voro(false, s);
       process_lower(s);
     }
   }
-  // update old distances (this is just an optimization)
+  // update old distances and obsts_size (this is just an optimization)
   pos_set all_updated_distances;
   all_updated_distances.insert(voro_to_true.begin(), voro_to_true.end());
   all_updated_distances.insert(voro_to_false.begin(), voro_to_false.end());
@@ -278,6 +340,12 @@ void GVDIC::update_obstacle_distance_map(vector<pos> obs_to_free,
     /* cout<<"  old_dist = "<<p_cd.dist_old<<endl; */
     /* cout<<"  dist = "<<p_cd.dist<<endl; */
     p_cd.dist_old = p_cd.dist;
+    p_cd.old_obsts_size = p_cd.obsts.size();
+  }
+  for(auto it = updated_obst_size.begin();it!=updated_obst_size.end();it++){
+    pos p = *it;
+    CellDataO &p_cd =cell_data_o[p];
+    p_cd.old_obsts_size = p_cd.obsts.size();
   }
 }
 
@@ -329,7 +397,7 @@ void GVDIC::set_consistent_borders(vector<pos> unk_to_free) {
     for (auto it = adj_s.begin(); it != adj_s.end(); it++) {
       pos n = (*it);
       CellDataO& n_cd = cell_data_o[n];
-      if (is_occ(n_cd.obsts,n) && !is_elem(inserted, s)) {
+      if (is_occ(n_cd,n) && !is_elem(inserted, s)) {
         // cout << " border:"<<n << endl;
         open.push(make_pair(n_cd.dist, n));
         inserted.insert(n);
@@ -492,10 +560,10 @@ void GVDIC::update_gvd() {
   /* print_property(voro_to_false,gt); */
   cout<<"distance_update"<<endl;
   print_property(distance_update,gt);
-  cout<<"obst_condition_change"<<endl;
-  print_property(obst_condition_change,gt);
+  cout<<"obst_cond_to_false"<<endl;
+  print_property(obst_cond_to_false,gt);
 
-  erosion_input.insert(obst_condition_change.begin(),obst_condition_change.end());
+  erosion_input.insert(obst_cond_to_false.begin(),obst_cond_to_false.end());
   erosion_input.insert(voro_to_true.begin(), voro_to_true.end());
   erosion_input.insert(distance_update.begin(), distance_update.end()); 
   boost::tie(to_add_gvd, to_erase_gvd) = erosion(erosion_input);
@@ -623,7 +691,7 @@ boost::tuple<criticals_info, GVD> GVDIC::get_points_of_interest(grid gt) {
   possible_crit_to_true.clear();
   possible_crit_to_false.clear();
 
-  obst_condition_change.clear();
+  obst_cond_to_false.clear();
   // increment
   this->gt = gt;
   vector<pos> obs_to_free, any_to_obs, unk_to_free, unk_to_notunk;

src/lib/GVD/GVDIC.h

@@ -16,7 +16,10 @@ class GVDIC {
   struct CellDataO {
     float dist;
     float dist_old;
+
     pos_set parents;
+
+    int old_obsts_size;
     pos_set obsts;
 
     bool to_raise;
@@ -53,7 +56,12 @@ class GVDIC {
   CellDataPQ open;
 
   // functions
-  bool is_occ(pos_set& obsts, pos p);
+  void set_obsts(CellDataO &cd, pos_set new_obsts, pos p);
+  void set_obsts(CellDataO &cd, vector<pos> new_obsts, pos p);
+  void insert_obsts(CellDataO &cd,pos_set new_obsts,pos p);
+  void insert_obsts(CellDataO &cd, vector<pos> new_obsts,pos p);
+
+  bool is_occ(CellDataO& cd, pos p);
   void check_voro(pos s, pos n);
 
   boost::tuple<vector<pos>, vector<pos>, vector<pos>, vector<pos>> get_changes(grid gt);