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
ad73d466
Commit
ad73d466
authored
Oct 18, 2020
by
Federico Ciuffardi
Browse files
obst_condition
parent
8c6f3c9b
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/lib/GVD/GVDIC.cpp
View file @
ad73d466
#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_cond
ition_chang
e
.
insert
(
p
);
obst_cond
_to_fals
e
.
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
()
);
s
et_
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_cond
ition_chang
e"
<<
endl
;
print_property
(
obst_cond
ition_chang
e
,
gt
);
cout
<<
"obst_cond
_to_fals
e"
<<
endl
;
print_property
(
obst_cond
_to_fals
e
,
gt
);
erosion_input
.
insert
(
obst_cond
ition_chang
e
.
begin
(),
obst_cond
ition_chang
e
.
end
());
erosion_input
.
insert
(
obst_cond
_to_fals
e
.
begin
(),
obst_cond
_to_fals
e
.
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_cond
ition_chang
e
.
clear
();
obst_cond
_to_fals
e
.
clear
();
// increment
this
->
gt
=
gt
;
vector
<
pos
>
obs_to_free
,
any_to_obs
,
unk_to_free
,
unk_to_notunk
;
...
...
src/lib/GVD/GVDIC.h
View file @
ad73d466
...
...
@@ -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
);
...
...
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