forked from ANYbotics/grid_map
-
Notifications
You must be signed in to change notification settings - Fork 0
/
filters_demo_filter_chain.yaml
126 lines (112 loc) · 4.08 KB
/
filters_demo_filter_chain.yaml
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
grid_map_filters:
- name: first
type: gridMapFilters/MockFilter
params:
processing_time: 100
print_name: true
- name: buffer_normalizer
type: gridMapFilters/BufferNormalizerFilter
# # Duplicate layer.
# - name: duplicate
# type: gridMapFilters/DuplicationFilter
# params:
# input_layer: ...
# output_layer: ...
# Delete color layer.
- name: delete_original_layers
type: gridMapFilters/DeletionFilter
params:
layers: [color] # List of layers.
# Fill holes in the map with inpainting.
- name: inpaint
type: gridMapCv/InpaintFilter
params:
input_layer: elevation
output_layer: elevation_inpainted
radius: 0.05
# Reduce noise with a radial blurring filter.
- name: mean_in_radius
type: gridMapFilters/MeanInRadiusFilter
params:
input_layer: elevation_inpainted
output_layer: elevation_smooth
radius: 0.06
# Boxblur as an alternative to the inpaint and radial blurring filter above.
# - name: boxblur
# type: gridMapFilters/SlidingWindowMathExpressionFilter
# params:
# input_layer: elevation
# output_layer: elevation_smooth
# expression: meanOfFinites(elevation)
# compute_empty_cells: true
# edge_handling: crop # options: inside, crop, empty, mean
# window_size: 5 # optional
# Compute surface normals.
- name: surface_normals
type: gridMapFilters/NormalVectorsFilter
params:
input_layer: elevation_inpainted
output_layers_prefix: normal_vectors_
radius: 0.05
normal_vector_positive_axis: z
# Add a color layer for visualization based on the surface normal.
- name: normal_color_map
type: gridMapFilters/NormalColorMapFilter
params:
input_layers_prefix: normal_vectors_
output_layer: normal_color
# Compute slope from surface normal.
- name: slope
type: gridMapFilters/MathExpressionFilter
params:
output_layer: slope
expression: acos(normal_vectors_z)
# Compute roughness as absolute difference from map to smoothened map.
- name: roughness
type: gridMapFilters/MathExpressionFilter
params:
output_layer: roughness
expression: abs(elevation_inpainted - elevation_smooth)
# Edge detection by computing the standard deviation from slope.
- name: edge_detection
type: gridMapFilters/SlidingWindowMathExpressionFilter
params:
input_layer: slope
output_layer: edges
expression: sqrt(sumOfFinites(square(slope - meanOfFinites(slope))) ./ numberOfFinites(slope)) # Standard deviation
compute_empty_cells: false
edge_handling: crop # options: inside, crop, empty, mean
window_length: 0.05
# # Edge detection on elevation layer with convolution filter as alternative to filter above.
# - name: edge_detection
# type: gridMapFilters/SlidingWindowMathExpressionFilter
# params:
# input_layer: elevation_inpainted
# output_layer: edges
# expression: 'sumOfFinites([0,1,0;1,-4,1;0,1,0].*elevation_inpainted)' # Edge detection.
# # expression: 'sumOfFinites([0,-1,0;-1,5,-1;0,-1,0].*elevation_inpainted)' # Sharpen.
# compute_empty_cells: false
# edge_handling: mean # options: inside, crop, empty, mean
# window_size: 3 # Make sure to make this compatible with the kernel matrix.
# Compute traversability as normalized weighted sum of slope and roughness.
- name: traversability
type: gridMapFilters/MathExpressionFilter
params:
output_layer: traversability
expression: 0.5 * (1.0 - (slope / 0.6)) + 0.5 * (1.0 - (roughness / 0.1))
# Set lower threshold on traversability.
- name: traversability_lower_threshold
type: gridMapFilters/ThresholdFilter
params:
condition_layer: traversability
output_layer: traversability
lower_threshold: 0.0
set_to: 0.0
# Set upper threshold on traversability.
- name: traversability_upper_threshold
type: gridMapFilters/ThresholdFilter
params:
condition_layer: traversability
output_layer: traversability
upper_threshold: 1.0
set_to: 1.0 # Other uses: .nan, .inf