void
convolve_prob_treemap( carmen_vascocore_quad_tree_t *tree,
carmen_vascocore_map_t *map,
carmen_gauss_kernel_t kernel, int hk )
{
int i, j;
if ((tree->level)>0 ) {
if (tree->elem[0]->inuse)
convolve_prob_treemap( tree->elem[0], map, kernel, hk );
if (tree->elem[1]->inuse)
convolve_prob_treemap( tree->elem[1], map, kernel, hk );
if (tree->elem[2]->inuse)
convolve_prob_treemap( tree->elem[2], map, kernel, hk );
if (tree->elem[3]->inuse)
convolve_prob_treemap( tree->elem[3], map, kernel, hk );
} else {
if ( (tree->center.x/2)>hk+1 && (tree->center.x/2)<map->mapsize.x-hk-1 &&
(tree->center.y/2)>hk+1 && (tree->center.y/2)<map->mapsize.y-hk-1 ) {
for (i=(tree->center.x/2)-hk;i<(tree->center.x/2)+hk;i++) {
for (j=(tree->center.y/2)-hk;j<(tree->center.y/2)+hk;j++) {
if (map->updated[i][j] != UPDT_Y)
convolve_prob_point( map, kernel, hk, i, j );
}
}
}
}
}
void
convolve_prob_treemap( QUAD_TREE *tree, MAP2 *map,
logtools_gauss_kernel_t kernel, int hk )
{
int i, j;
if ((tree->level)>0 ) {
if (tree->elem[0]->inuse)
convolve_prob_treemap( tree->elem[0], map, kernel, hk );
if (tree->elem[1]->inuse)
convolve_prob_treemap( tree->elem[1], map, kernel, hk );
if (tree->elem[2]->inuse)
convolve_prob_treemap( tree->elem[2], map, kernel, hk );
if (tree->elem[3]->inuse)
convolve_prob_treemap( tree->elem[3], map, kernel, hk );
} else {
if ( (tree->center.x/2)>hk+1 && (tree->center.x/2)<map->mapsize.x-hk-1 &&
(tree->center.y/2)>hk+1 && (tree->center.y/2)<map->mapsize.y-hk-1 ) {
for (i=(tree->center.x/2)-hk;i<(tree->center.x/2)+hk;i++) {
for (j=(tree->center.y/2)-hk;j<(tree->center.y/2)+hk;j++) {
if (map->updated[i][j] != UPDT_Y)
convolve_prob_point( map, kernel, hk, i, j );
}
}
}
}
}
void
convolve_treemap( carmen_vascocore_map_t *map )
{
int i;
static int hk, first_time = TRUE;
static carmen_gauss_kernel_t kernel;
if (first_time) {
hk = (carmen_vascocore_settings.local_map_kernel_len-1)/2;
kernel = carmen_gauss_kernel( carmen_vascocore_settings.local_map_kernel_len );
first_time = FALSE;
}
compute_prob_treemap( &(map->qtree), map );
for (i=0;i<carmen_vascocore_settings.local_map_num_convolve;i++) {
convolve_calc_treemap( &(map->qtree), map, kernel, hk,
carmen_vascocore_settings.local_map_std_val );
convolve_prob_treemap( &(map->qtree), map, kernel, hk );
}
}
void
convolve_map( MAP2 *map )
{
int i;
static int hk, first_time = TRUE;
static logtools_gauss_kernel_t kernel;
if (first_time) {
hk = (settings.local_map_kernel_len-1)/2;
kernel = logtools_compute_gauss_kernel( settings.local_map_kernel_len );
first_time = FALSE;
}
compute_prob_treemap( &(map->qtree), map );
for (i=0;i<settings.local_map_num_convolve;i++) {
convolve_calc_treemap( &(map->qtree), map, kernel, hk,
settings.local_map_std_val );
convolve_prob_treemap( &(map->qtree), map, kernel, hk );
}
}
