Commit 8c6f3c9b authored by Federico Ciuffardi's avatar Federico Ciuffardi
Browse files

fixes

parent 3d7b91b2
#include "GVDIC.h"
#include <cmath>
#include "pos.h"
#include "utils.h"
//////////
......@@ -24,9 +25,12 @@ 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
// 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) {
bool GVDIC::is_occ(pos_set& obsts, pos p) {
int old_size = obsts.size();
for (auto it = obsts.begin(); it != obsts.end();) {
pos obst = (*it);
if (gt[obst.first][obst.second] != Occupied) {
......@@ -35,11 +39,18 @@ bool GVDIC::is_occ(pos_set& obsts) {
++it;
}
}
int new_size = obsts.size();
/* cout<<p<<"("<<old_size<<","<<new_size<<")"<<endl; */
if ( old_size > 1 && new_size == 1){
/* cout<<"insert "<<p<<endl; */
obst_condition_change.insert(p);
}
return obsts.size() > 0;
}
// CellDataO
GVDIC::CellDataO::CellDataO() {
dist_old = MAXFLOAT;
dist = MAXFLOAT;
parents.insert(NULL_POS);
is_cleared = true;
......@@ -78,7 +89,7 @@ void GVDIC::CellDataC::clear_cell() {
//adds cell n to a set depending on value.
//this fuction builds sets of cells to add, to delete and to update the GVD
void GVDIC::set_voro(bool value, pos n) {
//cout<<"sv "<<n<<" : "<<value<<endl;
/* cout<<"sv "<<n<<" : "<<value<<endl; */
CellDataO& n_cd = cell_data_o[n];
// cout<<"Celda: "<<n<<" valor: "<<value<<endl;
if (value) {
......@@ -86,7 +97,11 @@ void GVDIC::set_voro(bool value, pos n) {
voro_to_true.insert(n);
}else{
// could check if the distance actually changes to avoid adding n
distance_update.insert(n);
if(n_cd.dist != n_cd.dist_old){
distance_update.insert(n);
}else{
distance_update.erase(n);
}
voro_to_false.erase(n);
}
} else {
......@@ -114,7 +129,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) && (obst_n != obst_s) && !is_elem(adj_n, obst_s)) {
if (is_occ(n_cd.obsts,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);
}
......@@ -154,7 +169,7 @@ void GVDIC::process_lower(pos s) {
n_cd.obsts.insert(s_closest_obsts.begin(), s_closest_obsts.end());
n_cd.parents.insert(s);
}
if (is_occ(n_cd.obsts)) {
if (is_occ(n_cd.obsts,n)) {
check_voro(s, n);
// cout << "check_voro" << n << endl;
}
......@@ -177,7 +192,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)) {
if (!is_occ(n_cd.obsts,n)) {
// cout << "raised: " <<n << endl;
open.push(make_pair(n_cd.dist, n));
n_cd.clear_cell();
......@@ -245,12 +260,25 @@ 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)) {
} else if (is_occ(s_cd.obsts,s)) {
// cout << "Process LOWER "<<s << endl;
set_voro(false, s);
process_lower(s);
}
}
// update old distances (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());
all_updated_distances.insert(distance_update.begin(), distance_update.end());
for(auto it = all_updated_distances.begin();it!=all_updated_distances.end();it++){
pos p = *it;
CellDataO &p_cd =cell_data_o[p];
/* cout<<p<<endl; */
/* cout<<" old_dist = "<<p_cd.dist_old<<endl; */
/* cout<<" dist = "<<p_cd.dist<<endl; */
p_cd.dist_old = p_cd.dist;
}
}
void GVDIC::update_frontiers(vector<pos> unk_to_notunk) {
......@@ -301,15 +329,15 @@ 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) && !is_elem(inserted, s)) {
if (is_occ(n_cd.obsts,n) && !is_elem(inserted, s)) {
// cout << " border:"<<n << endl;
open.push(make_pair(n_cd.dist, n));
inserted.insert(n);
}
}
}
cout << "consistent borders" << endl;
print_property(inserted, gt);
/* cout << "consistent borders" << endl; */
/* print_property(inserted, gt); */
}
void GVDIC::set_obstacle(pos p) {
......@@ -415,8 +443,8 @@ boost::tuple<pos_set, pos_set> GVDIC::erosion(pos_set voro_to_true) {
}
}
}
cout<<"erosion"<<endl;
print_property(inserted,gt);
/* cout<<"erosion"<<endl; */
/* print_property(inserted,gt); */
while (!erosion_queue.empty()) {
pos p = erosion_queue.top().second;
erosion_queue.pop();
......@@ -458,15 +486,20 @@ pos_set GVDIC::get_to_update_min(pos_set to_change){
void GVDIC::update_gvd() {
pos_set to_add_gvd, to_erase_gvd, to_update_gvd, to_update_min, erosion_input;
//cout<<"voro_to_true"<<endl;
//print_property(voro_to_true,gt);
//cout<<"voro_to_false"<<endl;
//print_property(voro_to_false,gt);
//cout<<"distance_update"<<endl;
//print_property(distance_update,gt);
/* cout<<"voro_to_true"<<endl; */
/* print_property(voro_to_true,gt); */
/* cout<<"voro_to_false"<<endl; */
/* 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);
erosion_input.insert(obst_condition_change.begin(),obst_condition_change.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);
to_erase_gvd.insert(voro_to_false.begin(), voro_to_false.end());
pos_set aux_min;
......@@ -590,6 +623,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();
// increment
this->gt = gt;
vector<pos> obs_to_free, any_to_obs, unk_to_free, unk_to_notunk;
......
......@@ -53,7 +53,7 @@ class GVDIC {
CellDataPQ open;
// functions
bool is_occ(pos_set& obsts);
bool is_occ(pos_set& obsts, pos p);
void check_voro(pos s, pos n);
boost::tuple<vector<pos>, vector<pos>, vector<pos>, vector<pos>> get_changes(grid gt);
......
......@@ -335,37 +335,31 @@ vector<grid> test_7 = {
vector<grid> test_8 = {
{
{Unknown, Unknown , Unknown , Unknown , Unknown , Unknown , Unknown, Unknown, Unknown},
{Unknown, Unknown , Unknown , Unknown , Unknown , Unknown , Unknown, Unknown, Unknown},
{Unknown, Unknown , Unknown , Unknown , Unknown , Unknown , Unknown, Unknown, Unknown},
{Unknown, Unknown , Unknown , Occupied, Occupied, Occupied, Free , Free , Unknown},
{Unknown, Unknown , Free , Free , Free , Free , Free , Free , Unknown},
{Occupied, Free , Free , Free , Free , Free , Free , Free , Unknown},
{Occupied, Free , Free , Free , Free , Free , Free , Free , Unknown},
{Occupied, Free , Free , Occupied, Occupied, Occupied, Unknown, Unknown, Unknown},
{Occupied, Occupied, Occupied, Occupied, Unknown , Unknown , Unknown, Unknown, Unknown},
{Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
{Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
},
{
{Unknown, Unknown , Unknown , Unknown , Unknown , Unknown , Unknown, Unknown, Unknown},
{Unknown, Unknown , Unknown , Unknown , Unknown , Unknown , Unknown, Unknown, Unknown},
{Unknown, Unknown , Unknown , Unknown , Unknown , Unknown , Unknown, Unknown, Unknown},
{Unknown, Unknown , Unknown , Occupied, Occupied, Occupied, Free , Free , Unknown},
{Unknown, Unknown , Free , Free , Occupied, Free , Free , Free , Unknown},
{Occupied, Free , Free , Free , Free , Free , Free , Free , Unknown},
{Occupied, Free , Free , Free , Occupied, Free , Free , Free , Unknown},
{Occupied, Free , Free , Occupied, Occupied, Occupied, Unknown, Unknown, Unknown},
{Occupied, Occupied, Occupied, Occupied, Unknown , Unknown , Unknown, Unknown, Unknown},
{Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
{Free , Free , Free , Free , Occupied, 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 , Occupied, Free , Free ,Free , Free },
{Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
},
{
{Unknown, Unknown , Unknown , Unknown , Unknown , Unknown , Unknown, Unknown, Unknown},
{Unknown, Unknown , Unknown , Unknown , Unknown , Unknown , Unknown, Unknown, Unknown},
{Unknown, Unknown , Unknown , Unknown , Unknown , Unknown , Unknown, Unknown, Unknown},
{Unknown, Unknown , Unknown , Occupied, Occupied, Occupied, Free , Free , Unknown},
{Unknown, Unknown , Free , Free , Free , Free , Free , Free , Unknown},
{Occupied, Free , Free , Free , Free , Free , Free , Free , Unknown},
{Occupied, Free , Free , Free , Free , Free , Free , Free , Unknown},
{Occupied, Free , Free , Occupied, Occupied, Occupied, Unknown, Unknown, Unknown},
{Occupied, Occupied, Occupied, Occupied, Unknown , Unknown , Unknown, Unknown, Unknown},
{Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
{Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
},
};
......
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