bool
InDoorOpeningDetector::inDoorOpening(const sensor_msgs::LaserScan &scan,
double minWidth, double maxWidth)
{
if (m_Algorithm == 2) return algorithm2(scan, minWidth, maxWidth);
else return algorithm1(scan, minWidth, maxWidth);
}
n_division* algorithm3 (sparse_matrix_arr *adj_matrix, double precision, int use_improve){
int n_groups = 0; /* counts number of groups in P */
int i, j;
int n = adj_matrix->n; /* number of vertices */
int_vector *is_divisable; /* indicated whether a group is divisable */
int_vector *vgroup;
int next_divisable_group;
int group_size;
two_division* division;
int n_div1; /* Marks the size of the +1 group in a two-division */
int_vector* p_groups;
n_division* res;
/* Allocate and Init res */
if ((p_groups = allocate_int_vector(n)) == NULL){
/* allocation failed */
return NULL;
}
if ((res = allocate_n_division(p_groups)) == NULL) {
/* allocation failed */
return NULL;
}
res->quality = 0.0;
for (i = 0; i < n; i++){
res->p_groups->values[i] = -1;
}
/* Allocate and Init is_divisable */
if ((is_divisable = allocate_int_vector(n+1)) == NULL){
/* allocation failed */
free_n_division(res);
return NULL;
}
/* First we want to remove singletons */
for (i = 0; i < n; ++i){
if (adj_matrix->rowptr[i] == adj_matrix->rowptr[i+1]){
res->p_groups->values[i] = n_groups;
is_divisable->values[n_groups] = 0;
++n_groups;
}
}
if (n_groups == n) {
next_divisable_group = -1;
}
else {
/* Now fill in the "last" group which is the group with every node
which isn't a singleton */
for (i = 0; i < n; i++) {
if (res->p_groups->values[i] == -1)
res->p_groups->values[i] = n_groups;
}
++n_groups;
is_divisable->values[n_groups-1] = 1;
next_divisable_group = n_groups-1;
}
while (next_divisable_group >= 0){
group_size = 0;
for (i = 0; i < n; i ++){
if (res->p_groups->values[i] == next_divisable_group){
group_size++;
}
}
if ((vgroup = allocate_int_vector(group_size)) == NULL){
/* allocation failed */
free_n_division(res);
free_int_vector(is_divisable);
return NULL;
}
/* filling vgroup with its relevant vertices */
j = 0;
for (i = 0; j < group_size; ++i){
if (res->p_groups->values[i] == next_divisable_group){
vgroup->values[j++] = i;
}
}
if ((division = algorithm2(adj_matrix, vgroup, precision, use_improve)) == NULL) {
/* Error in algorithm2! */
free_n_division(res);
free_int_vector(is_divisable);
free_int_vector(vgroup);
return NULL;
}
else {
/* Now we count how big is the +1 group */
n_div1 = 0;
for (i = 0; i < group_size; i++) {
n_div1 += (division->s_vector->values[i] > 0) ? 1 : 0;
}
if ((n_div1 == 0) || (n_div1 == group_size)) {
/* No division found... */
is_divisable->values[next_divisable_group] = 0;
}
else {
/* Woohoo! New division!
The division is a two-division, i.e. +1 / -1, and so we need
to screen the new groups using vgroup and the division we got */
res->quality += division->quality;
for (i = 0; i < group_size; i++) {
if (division->s_vector->values[i] > 0) {
res->p_groups->values[vgroup->values[i]] = n_groups;
}
}
n_groups++;
is_divisable->values[n_groups-1] = (n_div1 > 1) ? 1 : 0;
is_divisable->values[next_divisable_group] =
((group_size-n_div1) > 1) ? 1 : 0;
}
free_two_division(division);
}
/* Free the vgroup, there is no use of it anymore */
free_int_vector(vgroup);
/* Check if there are more divisable groups */
while ((next_divisable_group < n_groups) &&
(!is_divisable->values[next_divisable_group])) {
next_divisable_group++;
}
if (next_divisable_group == n_groups) next_divisable_group = -1;
}
free_int_vector(is_divisable);
return res;
}
