UFO 1.0.0
An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
Loading...
Searching...
No Matches
integrator.hpp
1
42#ifndef UFO_MAP_INTEGRATOR_INTEGRATOR_HPP
43#define UFO_MAP_INTEGRATOR_INTEGRATOR_HPP
44
45// UFO
46#include <ufo/cloud/label.hpp>
47#include <ufo/cloud/point_cloud.hpp>
48#include <ufo/container/tree/coord.hpp>
49#include <ufo/container/tree/index.hpp>
50#include <ufo/execution/algorithm.hpp>
51#include <ufo/execution/execution.hpp>
52#include <ufo/map/color/map.hpp>
53#include <ufo/map/occupancy/map.hpp>
54// #include <ufo/map/time/map.hpp>
55#include <ufo/map/integrator/count_sampling_method.hpp>
56#include <ufo/map/integrator/detail/bool_grid.hpp>
57#include <ufo/map/integrator/detail/count_grid.hpp>
58#include <ufo/map/integrator/detail/grid_map.hpp>
59#include <ufo/map/integrator/detail/hit.hpp>
60#include <ufo/map/integrator/detail/hit_grid.hpp>
61#include <ufo/map/integrator/detail/miss.hpp>
62#include <ufo/map/integrator/detail/miss_grid.hpp>
63#include <ufo/map/type.hpp>
64#include <ufo/map/void_region/map.hpp>
65#include <ufo/numeric/vec.hpp>
66#include <ufo/utility/spinlock.hpp>
67#include <ufo/utility/type_traits.hpp>
68
69// STL
70#include <cstddef>
71#include <type_traits>
72#include <vector>
73
74namespace ufo
75{
76enum class DownSamplingMethod { NONE, FIRST, CENTER };
77
78template <std::size_t Dim>
79class Integrator
80{
81 public:
82 //
83 // Tags
84 //
85 using occupancy_t = float;
86 using logit_t = float; // FIXME: Do not hardcode
87 using depth_t = unsigned;
88
89 depth_t hit_depth = 0;
90 depth_t miss_depth = 0;
91
92 occupancy_t occupancy_hit = 0.75f; // [0, 1]
93 occupancy_t occupancy_miss = 0.45f; // [0, 1]
94 logit_t occupancy_max_clamp_thres = 0.971f; // [0, 1]
95 logit_t occupancy_min_clamp_thres = 0.1192f; // [0, 1]
96
97 unsigned void_region_distance = 2;
98
99 DownSamplingMethod sample_method = DownSamplingMethod::NONE;
100
101 // TODO: Should this be here?
102 bool free_hits = false;
103
104 bool verbose = false;
105
106 CountSamplingMethod count_sample_method = CountSamplingMethod::NONE;
107
108 // A single hit in a void region will set the occupancy to max
109 bool void_region_instant_max_occupancy = true;
110 // A single miss in a void region will set the occupancy to min
111 bool void_region_instant_min_occupancy = true;
112
113 bool propagate = false;
114 bool prune = true;
115 bool reset_modified = true;
116
117 // TODO: WeightMode color_weight_mode = WeightMode::FIXED;
118
119 public:
120 template <class Map, class T, class... Rest>
121 void operator()(Map& map, PointCloud<Dim, T, Rest...> cloud,
122 Transform<Dim, T> const& transform = {}) const
123 {
124 // TODO: Implement
125 }
126
127 template <
128 class ExecutionPolicy, class Map, class T, class... Rest,
129 std::enable_if_t<execution::is_execution_policy_v<ExecutionPolicy>, bool> = true>
130 void operator()(ExecutionPolicy&& policy, Map& map, PointCloud<Dim, T, Rest...> cloud,
131 Transform<Dim, T> const& transform = {}) const
132 {
133 // TODO: Implement
134 }
135
136 protected:
137 /**************************************************************************************
138 | |
139 | Hits |
140 | |
141 **************************************************************************************/
142
143 template <class Map, class... Ts>
144 void insertHit(Map& map, TreeIndex const& node, SoAElement<Ts...> const& data,
145 logit_t occupancy, logit_t occupancy_min, logit_t occupancy_max) const
146 {
147 if constexpr (Map::hasMapTypes(MapType::OCCUPANCY)) {
148 if constexpr (Map::hasMapTypes(MapType::VOID_REGION)) {
149 if (void_region_instant_max_occupancy && map.voidRegion(node)) {
150 map.occupancyLogitSet(node, occupancy_max, false);
151 } else {
152 map.occupancyLogitUpdate(node, occupancy, false);
153 }
154 } else {
155 map.occupancyLogitUpdate(node, occupancy, false);
156 }
157 }
158
159 if constexpr (Map::hasMapTypes(MapType::COLOR) && contains_color_v<Ts...>) {
160 using color_type = first_color_t<Ts...>;
161
162 // TODO: Do something with weight (depending on `color_weight_mode`)
163
164 auto c = data.template get<color_type>();
165 map.colorAdd(node, c, false);
166 }
167
168 // TODO: Add more map types
169 }
170
171 template <class Map, class T, class... Rest>
172 void insertHits(Map& map, PointCloud<Dim, T, Rest...> const& cloud) const
173 {
174 auto const occ = probabilityToLogit(occupancy_hit);
175 auto const occ_min = probabilityToLogit(occupancy_min_clamp_thres);
176 auto const occ_max = probabilityToLogit(occupancy_max_clamp_thres);
177
178 cached_hits_.resize(cloud.size());
179 auto points = cloud.template view<0>();
180 ufo::transform(
181 points.begin(), points.end(), cached_hits_.begin(),
182 [&map, d = hit_depth](auto const& p) { return map.code(TreeCoord(p, d)); });
183
184 map.create(cached_hits_, cached_hits_.begin());
185
186 ufo::for_each(std::size_t(0), static_cast<std::size_t>(cached_hits_.size()),
187 [this, &map, &cloud, occ, occ_min, occ_max](std::size_t i) {
188 auto node = cached_hits_[i].node;
189
190 // This chick wants to rule the block (node.pos being the block)
191 // std::lock_guard lock(map.chicken(node.pos));
192
193 insertHit(map, node, cloud[i], occ, occ_min, occ_max);
194 });
195 }
196
197 template <
198 class ExecutionPolicy, class Map, class T, class... Rest,
199 std::enable_if_t<execution::is_execution_policy_v<ExecutionPolicy>, bool> = true>
200 void insertHits(ExecutionPolicy&& policy, Map& map,
201 PointCloud<Dim, T, Rest...> const& cloud) const
202 {
203 auto const occ = probabilityToLogit(occupancy_hit);
204 auto const occ_min = probabilityToLogit(occupancy_min_clamp_thres);
205 auto const occ_max = probabilityToLogit(occupancy_max_clamp_thres);
206
207 cached_hits_.resize(cloud.size());
208 auto points = cloud.template view<0>();
209 ufo::transform(
210 policy, points.begin(), points.end(), cached_hits_.begin(),
211 [&map, d = hit_depth](auto const& p) { return map.code(TreeCoord(p, d)); });
212
213 map.create(policy, cached_hits_, cached_hits_.begin());
214
215 ufo::for_each(std::forward<ExecutionPolicy>(policy), std::size_t(0),
216 static_cast<std::size_t>(cached_hits_.size()),
217 [this, &map, &cloud, occ, occ_min, occ_max](std::size_t i) {
218 auto node = cached_hits_[i].node;
219
220 // This chick wants to rule the block (node.pos being the block)
221 // std::lock_guard lock(map.chicken(node.pos));
222
223 insertHit(map, node, cloud[i], occ, occ_min, occ_max);
224 });
225 }
226
227 template <class Map, class T, class... Ts>
228 void insertHit(Map& map, Vec<Dim, T> const& sensor_origin, TreeIndex const& node,
229 SoAElement<Ts...> const& data, logit_t occupancy, logit_t occupancy_min,
230 logit_t occupancy_max) const
231 {
232 if constexpr (Map::hasMapTypes(MapType::OCCUPANCY)) {
233 if constexpr (Map::hasMapTypes(MapType::VOID_REGION)) {
234 if (void_region_instant_max_occupancy && map.voidRegion(node)) {
235 map.occupancyLogitSet(node, occupancy_max, false);
236 } else {
237 map.occupancyLogitUpdate(node, occupancy, false);
238 }
239 } else {
240 map.occupancyLogitUpdate(node, occupancy, false);
241 }
242 }
243
244 if constexpr (Map::hasMapTypes(MapType::COLOR) && contains_color_v<Ts...>) {
245 using color_type = first_color_t<Ts...>;
246
247 // TODO: Do something with weight (depending on `color_weight_mode`)
248
249 auto c = data.template get<color_type>();
250 if constexpr (map.colorDirectional()) {
251 auto dir = data.template get<0>() - sensor_origin;
252 map.colorAdd(node, normalize(dir), c, false);
253 } else {
254 map.colorAdd(node, c, false);
255 }
256 }
257
258 // TODO: Add more map types
259 }
260
261 template <class Map, class T, class... Rest>
262 void insertHits(Map& map, PointCloud<Dim, T, Rest...> const& cloud,
263 Vec<Dim, T> const& sensor_origin) const
264 {
265 auto const occ = probabilityToLogit(occupancy_hit);
266 auto const occ_min = probabilityToLogit(occupancy_min_clamp_thres);
267 auto const occ_max = probabilityToLogit(occupancy_max_clamp_thres);
268
269 cached_hits_.resize(cloud.size());
270 auto points = cloud.template view<0>();
271 ufo::transform(
272 points.begin(), points.end(), cached_hits_.begin(),
273 [&map, d = hit_depth](auto const& p) { return map.code(TreeCoord(p, d)); });
274
275 map.create(cached_hits_, cached_hits_.begin());
276
277 ufo::for_each(
278 std::size_t(0), static_cast<std::size_t>(cached_hits_.size()),
279 [this, &map, &cloud, &sensor_origin, occ, occ_min, occ_max](std::size_t i) {
280 auto node = cached_hits_[i].node;
281
282 // This chick wants to rule the block (node.pos being the block)
283 // std::lock_guard lock(map.chicken(node.pos));
284
285 insertHit(map, sensor_origin, node, cloud[i], occ, occ_min, occ_max);
286 });
287 }
288
289 template <
290 class ExecutionPolicy, class Map, class T, class... Rest,
291 std::enable_if_t<execution::is_execution_policy_v<ExecutionPolicy>, bool> = true>
292 void insertHits(ExecutionPolicy&& policy, Map& map,
293 PointCloud<Dim, T, Rest...> const& cloud,
294 Vec<Dim, T> const& sensor_origin) const
295 {
296 auto const occ = probabilityToLogit(occupancy_hit);
297 auto const occ_min = probabilityToLogit(occupancy_min_clamp_thres);
298 auto const occ_max = probabilityToLogit(occupancy_max_clamp_thres);
299
300 cached_hits_.resize(cloud.size());
301 auto points = cloud.template view<0>();
302 ufo::transform(
303 policy, points.begin(), points.end(), cached_hits_.begin(),
304 [&map, d = hit_depth](auto const& p) { return map.code(TreeCoord(p, d)); });
305
306 map.create(policy, cached_hits_, cached_hits_.begin());
307
308 ufo::for_each(
309 std::forward<ExecutionPolicy>(policy), std::size_t(0),
310 static_cast<std::size_t>(cached_hits_.size()),
311 [this, &map, &cloud, &sensor_origin, occ, occ_min, occ_max](std::size_t i) {
312 auto node = cached_hits_[i].node;
313
314 // This chick wants to rule the block (node.pos being the block)
315 // std::lock_guard lock(map.chicken(node.pos));
316
317 insertHit(map, sensor_origin, node, cloud[i], occ, occ_min, occ_max);
318 });
319 }
320
321 /**************************************************************************************
322 | |
323 | Misses |
324 | |
325 **************************************************************************************/
326
327 template <class Map>
328 void insertMiss(Map& map, detail::Miss<Dim> const& miss, logit_t occupancy,
329 logit_t occupancy_min, logit_t occupancy_max) const
330 {
331 if constexpr (Map::hasMapTypes(MapType::VOID_REGION)) {
332 if (miss.void_region) {
333 map.voidRegionSet(miss.node, true, false);
334 }
335 }
336
337 if constexpr (Map::hasMapTypes(MapType::OCCUPANCY)) {
338 // TODO: Is this good?
339 if constexpr (Map::hasMapTypes(MapType::VOID_REGION)) {
340 if (void_region_instant_min_occupancy && map.voidRegion(miss.node)) {
341 map.occupancyLogitSet(miss.node, occupancy_min, false);
342 } else {
343 map.occupancyLogitUpdate(miss.node, miss.count * occupancy, false);
344 }
345 } else {
346 map.occupancyLogitUpdate(miss.node, miss.count * occupancy, false);
347 }
348 }
349
350 // TODO: Add more map types
351 }
352
353 template <class Map>
354 void insertMisses(Map& map, std::vector<detail::Miss<Dim>>& misses) const
355 {
356 auto const occ = probabilityToLogit(occupancy_miss);
357 auto const occ_min = probabilityToLogit(occupancy_min_clamp_thres);
358 auto const occ_max = probabilityToLogit(occupancy_max_clamp_thres);
359
360 map.create(misses, misses.begin());
361
362 ufo::for_each(misses.begin(), misses.end(),
363 [this, &map, occ, occ_min, occ_max](auto const& miss) {
364 // This chick wants to rule the block (node.pos being the block)
365 // std::lock_guard lock(map.chicken(miss.index));
366
367 insertMiss(map, miss, occ, occ_min, occ_max);
368 });
369 }
370
371 template <
372 class ExecutionPolicy, class Map,
373 std::enable_if_t<execution::is_execution_policy_v<ExecutionPolicy>, bool> = true>
374 void insertMisses(ExecutionPolicy&& policy, Map& map,
375 std::vector<detail::Miss<Dim>>& misses) const
376 {
377 auto const occ = probabilityToLogit(occupancy_miss);
378 auto const occ_min = probabilityToLogit(occupancy_min_clamp_thres);
379 auto const occ_max = probabilityToLogit(occupancy_max_clamp_thres);
380
381 map.create(std::forward<ExecutionPolicy>(policy), misses, misses.begin());
382
383 ufo::for_each(std::forward<ExecutionPolicy>(policy), misses.begin(), misses.end(),
384 [this, &map, occ, occ_min, occ_max](auto const& miss) {
385 // This chick wants to rule the block (node.pos being the block)
386 // std::lock_guard lock(map.chicken(miss.index));
387
388 insertMiss(map, miss, occ, occ_min, occ_max);
389 });
390 }
391
392 protected:
393 mutable __block std::vector<detail::Hit<Dim>> cached_hits_;
394};
395} // namespace ufo
396
397#endif // UFO_MAP_INTEGRATOR_INTEGRATOR_HPP
ufo::probabilityToLogit
constexpr T probabilityToLogit(T probability)
Converts probability to logit value.
Definition math.hpp:129
ufo
All vision-related classes and functions.
Definition cloud.hpp:49
ufo::for_each
constexpr UnaryFunc for_each(Index first, Index last, UnaryFunc f, Proj proj={})
Applies f to each integer index in [first, last) sequentially.
Definition algorithm.hpp:80
ufo::normalize
constexpr Quat< T > normalize(Quat< T > const &q) noexcept
Returns a unit quaternion in the same direction as q.
Definition quat.hpp:679
ufo::transform
PointCloud< Dim, T, Rest... > transform(Transform< Dim, T > const &t, PointCloud< Dim, T, Rest... > pc)
Applies a rigid transform to every point position and returns the result.
Definition point_cloud.hpp:105
ufo::Transform
Rigid-body transform: a rotation matrix plus a translation vector.
Definition transform.hpp:81
ufo::Vec
A fixed-size arithmetic vector of up to 4 dimensions.
Definition vec.hpp:76