UFO 1.0.0
An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
Loading...
Searching...
No Matches
occupancy.hpp
1
42#ifndef UFO_MAP_OCCUPANCY_PREDICATE_OCCUPANCY_HPP
43#define UFO_MAP_OCCUPANCY_PREDICATE_OCCUPANCY_HPP
44
45// UFO
46#include <ufo/container/tree/index.hpp>
47#include <ufo/container/tree/predicate/filter.hpp>
48#include <ufo/container/tree/predicate/predicate_compare.hpp>
49#include <ufo/map/occupancy/propagation_criteria.hpp>
50
51// STL
52#include <cassert>
53
54namespace ufo::pred
55{
56template <PredicateCompare PC = PredicateCompare::EQUAL>
57struct Occupancy {
58 using occupancy_t = float;
59 using logit_t = std::int8_t;
60
61 Occupancy(occupancy_t occupancy) : occupancy(occupancy) {}
62
63 occupancy_t occupancy;
64
65 protected:
66 logit_t logit;
67
68 template <class T>
69 friend class Filter;
70};
71
72using OccupancyE = Occupancy<>;
73using OccupancyNE = Occupancy<PredicateCompare::NOT_EQUAL>;
74using OccupancyLE = Occupancy<PredicateCompare::LESS_EQUAL>;
75using OccupancyGE = Occupancy<PredicateCompare::GREATER_EQUAL>;
76using OccupancyL = Occupancy<PredicateCompare::LESS>;
77using OccupancyG = Occupancy<PredicateCompare::GREATER>;
78
79using OccupancyMin = OccupancyGE;
80using OccupancyMax = OccupancyLE;
81
82// TODO: Implement this
83// pred::Filter::init(p, t);
84// pred::Filter::returnable(p, t, n);
85// pred::Filter::traversable(p, t, n);
86
87// template <PredicateCompare PC>
88// struct Filter<Occupancy<PC>> {
89// using Pred = Occupancy<PC>;
90
91// template <class Tree>
92// static constexpr void init(Pred& p, Tree const& t)
93// {
94// p.logit = t.occupancyLogit(p.occupancy);
95// }
96
97// template <class Tree>
98// [[nodiscard]] static constexpr bool returnable(Pred p, Tree const& t, TreeIndex n)
99// {
100// if constexpr (PredicateCompare::EQUAL == PC) {
101// return t.occupancyLogit(n) == p.logit;
102// } else if constexpr (PredicateCompare::NOT_EQUAL == PC) {
103// return t.occupancyLogit(n) != p.logit;
104// } else if constexpr (PredicateCompare::LESS_EQUAL == PC) {
105// return t.occupancyLogit(n) <= p.logit;
106// } else if constexpr (PredicateCompare::GREATER_EQUAL == PC) {
107// return t.occupancyLogit(n) >= p.logit;
108// } else if constexpr (PredicateCompare::LESS == PC) {
109// return t.occupancyLogit(n) < p.logit;
110// } else if constexpr (PredicateCompare::GREATER == PC) {
111// return t.occupancyLogit(n) > p.logit;
112// }
113// }
114
115// template <class Tree>
116// [[nodiscard]] static constexpr bool traversable(Pred p, Tree const& t, TreeIndex n)
117// {
118// switch (t.occupancyPropagationCriteria()) {
119// case OccupancyPropagationCriteria::MIN:
120// if constexpr (PredicateCompare::EQUAL == PC) {
121// return t.occupancyLogit(n) <= p.logit;
122// } else if constexpr (PredicateCompare::NOT_EQUAL == PC) {
123// return true;
124// } else if constexpr (PredicateCompare::LESS_EQUAL == PC) {
125// return t.occupancyLogit(n) <= p.logit;
126// } else if constexpr (PredicateCompare::GREATER_EQUAL == PC) {
127// return true;
128// } else if constexpr (PredicateCompare::LESS == PC) {
129// return t.occupancyLogit(n) < p.logit;
130// } else if constexpr (PredicateCompare::GREATER == PC) {
131// return true;
132// }
133// case OccupancyPropagationCriteria::MAX:
134// if constexpr (PredicateCompare::EQUAL == PC) {
135// return t.occupancyLogit(n) >= p.logit;
136// } else if constexpr (PredicateCompare::NOT_EQUAL == PC) {
137// return true;
138// } else if constexpr (PredicateCompare::LESS_EQUAL == PC) {
139// return true;
140// } else if constexpr (PredicateCompare::GREATER_EQUAL == PC) {
141// return t.occupancyLogit(n) >= p.logit;
142// } else if constexpr (PredicateCompare::LESS == PC) {
143// return true;
144// } else if constexpr (PredicateCompare::GREATER == PC) {
145// return t.occupancyLogit(n) > p.logit;
146// }
147// case OccupancyPropagationCriteria::MEAN: return true;
148// case OccupancyPropagationCriteria::ONLY_INDICATORS: return true;
149// case OccupancyPropagationCriteria::NONE: return true;
150// }
151// assert(false);
152// return true;
153// }
154// };
155
156template <PredicateCompare PC>
157struct Filter<Occupancy<PC>> : public FilterBase<Occupancy<PC>> {
158 using Pred = Occupancy<PC>;
159
160 template <class Tree>
161 static constexpr void init(Pred& p, Tree const& t)
162 {
163 p.logit = t.occupancyLogit(p.occupancy);
164 }
165
166 template <class Tree>
167 [[nodiscard]] static constexpr bool returnable(Pred const& p, Tree const& t,
168 TreeIndex n)
169 {
170 if constexpr (PredicateCompare::EQUAL == PC) {
171 return t.occupancyLogit(n) == p.logit;
172 } else if constexpr (PredicateCompare::NOT_EQUAL == PC) {
173 return t.occupancyLogit(n) != p.logit;
174 } else if constexpr (PredicateCompare::LESS_EQUAL == PC) {
175 return t.occupancyLogit(n) <= p.logit;
176 } else if constexpr (PredicateCompare::GREATER_EQUAL == PC) {
177 return t.occupancyLogit(n) >= p.logit;
178 } else if constexpr (PredicateCompare::LESS == PC) {
179 return t.occupancyLogit(n) < p.logit;
180 } else if constexpr (PredicateCompare::GREATER == PC) {
181 return t.occupancyLogit(n) > p.logit;
182 }
183 }
184
185 template <class Tree>
186 [[nodiscard]] static constexpr bool traversable(Pred const& p, Tree const& t,
187 TreeIndex n)
188 {
189 switch (t.occupancyPropagationCriteria()) {
190 case OccupancyPropagationCriteria::MIN:
191 if constexpr (PredicateCompare::EQUAL == PC) {
192 return t.occupancyLogit(n) <= p.logit;
193 } else if constexpr (PredicateCompare::NOT_EQUAL == PC) {
194 return true;
195 } else if constexpr (PredicateCompare::LESS_EQUAL == PC) {
196 return t.occupancyLogit(n) <= p.logit;
197 } else if constexpr (PredicateCompare::GREATER_EQUAL == PC) {
198 return true;
199 } else if constexpr (PredicateCompare::LESS == PC) {
200 return t.occupancyLogit(n) < p.logit;
201 } else if constexpr (PredicateCompare::GREATER == PC) {
202 return true;
203 }
204 case OccupancyPropagationCriteria::MAX:
205 if constexpr (PredicateCompare::EQUAL == PC) {
206 return t.occupancyLogit(n) >= p.logit;
207 } else if constexpr (PredicateCompare::NOT_EQUAL == PC) {
208 return true;
209 } else if constexpr (PredicateCompare::LESS_EQUAL == PC) {
210 return true;
211 } else if constexpr (PredicateCompare::GREATER_EQUAL == PC) {
212 return t.occupancyLogit(n) >= p.logit;
213 } else if constexpr (PredicateCompare::LESS == PC) {
214 return true;
215 } else if constexpr (PredicateCompare::GREATER == PC) {
216 return t.occupancyLogit(n) > p.logit;
217 }
218 case OccupancyPropagationCriteria::MEAN: return true;
219 case OccupancyPropagationCriteria::ONLY_INDICATORS: return true;
220 case OccupancyPropagationCriteria::NONE: return true;
221 }
222 assert(false);
223 return true;
224 }
225};
226} // namespace ufo::pred
227
228#endif // UFO_MAP_OCCUPANCY_PREDICATE_OCCUPANCY_HPP
ufo::Tree
Utilizing curiously recurring template pattern (CRTP)
Definition tree.hpp:104