Commit ad73d466 authored by Federico Ciuffardi's avatar Federico Ciuffardi
Browse files

obst_condition

parent 8c6f3c9b
#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;
......
......@@ -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);
......
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