Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
Federico Ciuffardi Alves
pgMappingCooperativo
Commits
c5646a01
Commit
c5646a01
authored
Oct 20, 2020
by
Federico Ciuffardi
Browse files
bug fix on check_voro condition
parent
8ba93364
Changes
3
Hide whitespace changes
Inline
Side-by-side
src/lib/GVD/GVD.h
View file @
c5646a01
...
...
@@ -303,6 +303,7 @@ struct collapsedGraph : genericGraph<graph> {
//cout<<"ENTRE AL ERASE CON ALGO MAL: "<<p<<endl;
return
;
}
// cout<<"erasing "<<p<<endl;
//cout<<"ENTRO AL ERASE V CON P: "<<p<<endl;
// 0. erase from uncollapsed
full_graph
.
erase
(
p
);
...
...
@@ -345,6 +346,7 @@ struct collapsedGraph : genericGraph<graph> {
//cout<<"ENTRE AL ADD CON ALGO MAL"<<endl;
return
;
}
// cout<<"adding "<<p<<endl;
// 0. uncolloapse collapsed neighbors of p
//cout<<"PASO 0 CON P: "<<p<<endl;
...
...
src/lib/GVD/GVDIC.cpp
View file @
c5646a01
...
...
@@ -121,7 +121,7 @@ void GVDIC::CellDataO::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
)
{
...
...
@@ -159,15 +159,30 @@ void GVDIC::check_voro(pos s, pos n) {
if
(
s_cd
.
obsts
.
size
()
>
1
)
{
set_voro
(
true
,
s
);
}
pos
obst_n
=
*
n_cd
.
obsts
.
begin
();
pos
obst_s
=
*
s_cd
.
obsts
.
begin
();
auto
adj_n
=
adj
(
gt
,
obst_n
);
// pos obst_n = *n_cd.obsts.begin();
// pos obst_s = *s_cd.obsts.begin();
// auto adj_n = adj(gt, obst_n);
//cout<<"n "<<n<<"| s "<<s<<endl;
//cout<<" (is_occ(n_cd,n) && (obst_n != obst_s) && !is_elem(adj_n, obst_s))"<<endl;
//cout<<" "<<is_occ(n_cd,n) << " " << (obst_n != obst_s) <<" "<< !is_elem(adj_n, obst_s)<<endl; //beforej
if
(
is_occ
(
n_cd
,
n
)
&&
(
n_cd
.
obsts
!=
s_cd
.
obsts
))
{
// skip if all obstacles from s are adjacent to one or more obstacle from n
bool
are_adj
=
true
;
for
(
auto
it
=
s_cd
.
obsts
.
begin
();
it
!=
s_cd
.
obsts
.
end
()
&&
are_adj
;
it
++
){
pos
obst_s
=
*
it
;
for
(
auto
it
=
n_cd
.
obsts
.
begin
();
it
!=
n_cd
.
obsts
.
end
()
&&
are_adj
;
it
++
){
pos
obst_n
=
*
it
;
auto
adj_n
=
adj
(
gt
,
obst_n
);
are_adj
=
is_elem
(
adj_n
,
obst_s
);
}
}
if
(
are_adj
)
return
;
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
)
{
if
(
s_cd
.
dist
>
0
)
{
//(dist(s, obst_n) <= dist(n, obst_s)) && could add the multi obstacle variation
set_voro
(
true
,
s
);
}
if
((
dist
(
n
,
obst_s
)
<=
dist
(
s
,
obst_n
))
&&
n_cd
.
dist
>
0
)
{
if
(
n_cd
.
dist
>
0
)
{
//
(dist(n, obst_s) <= dist(s, obst_n)) &&
could add the multi obstacle variation
set_voro
(
true
,
n
);
}
}
...
...
@@ -478,25 +493,27 @@ boost::tuple<pos_set, pos_set> GVDIC::erosion(pos_set erosion_input) {
pos_set
adj_p
=
adj
(
gt
,
p
);
for
(
auto
it
=
adj_p
.
begin
();
it
!=
adj_p
.
end
();
it
++
)
{
pos
n
=
*
it
;
if
(
!
is_elem
(
inserted
,
n
)
&&
cell_data_o
[
n
].
is_voro
)
{
if
(
!
is_elem
(
inserted
,
n
)
&&
cell_data_o
[
n
].
is_voro
)
{
inserted
.
insert
(
n
);
erosion_queue
.
push
(
make_pair
(
cell_data_o
[
n
].
dist
,
n
));
}
}
}
//
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
();
// cout<<p<<" obsts "<<cell_data_o[p].obsts.size() <<"| A " <<A(p) <<endl;
if
(
cell_data_o
[
p
].
obsts
.
size
()
<
2
&&
A
(
p
)
<=
1
)
{
// cout<<p<<"->eroded"<<endl;
cell_data_o
[
p
].
is_voro
=
false
;
if
(
is_elem
(
gvd
.
full_graph
,
p
)){
distance_update
.
erase
(
p
);
to_erase
.
insert
(
p
);
}
}
else
{
// cout<<p<<"->inserted"<<endl;
//if is !in full
if
(
!
is_elem
(
gvd
.
full_graph
,
p
)){
to_add
.
insert
(
p
);
...
...
@@ -528,10 +545,16 @@ void GVDIC::update_gvd() {
pos_set
to_add_gvd
,
to_erase_gvd
,
to_update_gvd
,
to_update_min
,
erosion_input
;
// erosion input
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
());
// as old_region_border cells must go from true to false (and not otherwise) we filter out the true ones
/// as old_region_border and obst_cond_to_false cells must go from true to false
/// (and not otherwise) we filter out the true ones
for
(
auto
it
=
obst_cond_to_false
.
begin
();
it
!=
obst_cond_to_false
.
end
();
it
++
){
pos
p
=
*
it
;
if
(
cell_data_o
[
p
].
is_voro
){
erosion_input
.
insert
(
p
);
}
}
for
(
auto
it
=
old_region_border
.
begin
();
it
!=
old_region_border
.
end
();
it
++
){
pos
p
=
*
it
;
if
(
cell_data_o
[
p
].
is_voro
){
...
...
@@ -553,14 +576,14 @@ void GVDIC::update_gvd() {
// print_property(voro_to_true,gt);
// cout<<"old_region_border"<<endl;
// print_property(old_region_border,gt);
//cout<<"voro_to_false"<<endl;
//
cout<<"voro_to_false"<<endl;
// print_property(voro_to_false,gt);
// cout<<"distance_update"<<endl;
// print_property(distance_update,gt);
// cout<<"obst_cond_to_false"<<endl;
// print_property(obst_cond_to_false,gt);
//cout<<"erosion_input"<<endl;
//print_property(erosion_input,gt);
//
cout<<"erosion_input"<<endl;
//
print_property(erosion_input,gt);
// erosion
boost
::
tie
(
to_add_gvd
,
to_erase_gvd
)
=
erosion
(
erosion_input
);
...
...
src/lib/GVD/test.cpp
View file @
c5646a01
...
...
@@ -422,40 +422,40 @@ vector<grid> test_9 = {
vector
<
grid
>
test_10
=
{
{
{
Occupied
,
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
,
Free
,
Free
,
Free
,
Free
,
Free
},
{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
{
Unknown
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
{
Unknown
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
},
{
Unknown
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
},
{
Unknown
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
},
{
Unknown
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
},
{
Unknown
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
},
{
Unknown
,
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
},
{
Unknown
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
{
Unknown
,
Free
,
Free
,
Free
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Unknown
,
Free
,
Free
,
Free
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Unknown
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
},
{
Unknown
,
Free
,
Free
,
Free
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Unknown
,
Free
,
Free
,
Free
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Unknown
,
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
,
Occupied
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
},
{
Free
,
Free
,
Occupied
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Free
,
Free
,
Occupied
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
{
Unknown
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
{
Unknown
,
Free
,
Free
,
Free
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Unknown
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Unknown
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
},
{
Unknown
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Unknown
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Unknown
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
},
{
{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
{
Free
,
Free
,
Occupied
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Free
,
Free
,
Free
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Free
,
Free
,
Occupied
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Free
,
Free
,
Occupied
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Free
,
Free
,
Occupied
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
{
Unknown
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
{
Unknown
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Unknown
,
Free
,
Free
,
Free
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Unknown
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Unknown
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Unknown
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Unknown
,
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
};
...
...
@@ -491,7 +491,14 @@ int main(int argc, char** argv) {
GVDIC
igvdc
;
cout
<<
"# INITIALIZATION DONE"
<<
endl
;
// cout<<"DIST_NEW: "<< igvdc.cell_data_o[pos(0,0)].dist << endl;
for
(
int
i
=
0
;
i
<
test
[
0
].
size
();
i
++
)
{
for
(
int
j
=
0
;
j
<
test
[
0
][
i
].
size
();
j
++
)
{
pos
p
=
pos
(
i
,
j
);
cout
<<
"|"
<<
std
::
setfill
(
' '
)
<<
std
::
setw
(
7
)
<<
p
;
}
cout
<<
"|"
<<
endl
;
}
cout
<<
endl
;
for
(
int
t
=
0
;
t
<
test
.
size
();
t
++
)
{
cout
<<
endl
<<
"# get_points_of_interest "
<<
t
<<
endl
;
igvdc
.
get_points_of_interest
(
test
[
t
]);
...
...
@@ -579,19 +586,19 @@ int main(int argc, char** argv) {
cout
<<
"|"
<<
endl
;
}
//
cout << endl << "# Posible Critics distance map " << t << endl;
//
for (int i = 0; i < test[t].size(); i++) {
//
for (int j = 0; j < test[t][i].size(); j++) {
//
float d = igvdc.disMapIC->cell_data[pos(i, j)].dist;
//
if (d == MAXFLOAT) {
//
cout << "| ∞";
//
} else {
//
cout << "|" << std::setfill(' ') << std::setw(7) << d;
//
}
//
}
//
cout << "|" << endl;
//
}
//
cout << endl;
//
}
cout
<<
endl
<<
"# Posible Critics distance map "
<<
t
<<
endl
;
for
(
int
i
=
0
;
i
<
test
[
t
].
size
();
i
++
)
{
for
(
int
j
=
0
;
j
<
test
[
t
][
i
].
size
();
j
++
)
{
float
d
=
igvdc
.
disMapIC
->
cell_data
[
pos
(
i
,
j
)].
dist
;
if
(
d
==
MAXFLOAT
)
{
cout
<<
"| ∞"
;
}
else
{
cout
<<
"|"
<<
std
::
setfill
(
' '
)
<<
std
::
setw
(
7
)
<<
d
;
}
}
cout
<<
"|"
<<
endl
;
}
cout
<<
endl
;
}
return
0
;
}
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment