Line data Source code
1 : #include "ObjectInterpreter.hpp"
2 : #include <iterator>
3 : #include <utility>
4 : #include <boost/filesystem.hpp>
5 : #include <boost/filesystem/fstream.hpp>
6 : #include <boost/make_shared.hpp>
7 : #include <boost/range/algorithm/find_if.hpp>
8 : #include <boost/range/algorithm/sort.hpp>
9 : #include <boost/range/algorithm/transform.hpp>
10 : #include <CGAL/convex_hull_2.h>
11 : #include <pcl/PointIndices.h>
12 : #include <pcl/io/pcd_io.h>
13 : #include <pcl/octree/octree.h>
14 : #include <pcl/octree/octree_impl.h>
15 : #include <pcl/octree/octree_search.h>
16 : #include <pcl/point_types.h>
17 : #include <pcl/search/kdtree.h>
18 : #include <pcl/segmentation/extract_clusters.h>
19 : #include <pcl/visualization/cloud_viewer.h>
20 :
21 : namespace HLR
22 : {
23 : namespace PointCloud
24 : {
25 : namespace
26 : {
27 18 : long get_milliseconds_since_epoch()
28 : {
29 36 : return std::chrono::duration_cast<std::chrono::milliseconds>(
30 36 : std::chrono::time_point_cast<std::chrono::milliseconds>(
31 36 : std::chrono::system_clock::now())
32 36 : .time_since_epoch())
33 36 : .count();
34 : }
35 : }
36 :
37 16 : ObjectInterpreter::ObjectInterpreter()
38 16 : : last_measure_time(get_milliseconds_since_epoch())
39 16 : {}
40 :
41 4 : std::vector<Object> ObjectInterpreter::determine_speed(
42 : std::vector<Object>& objects,
43 : long milliseconds_since_epoch /*= -1*/)
44 : {
45 4 : if (milliseconds_since_epoch == -1)
46 : {
47 2 : milliseconds_since_epoch = get_milliseconds_since_epoch();
48 : }
49 :
50 4 : if (last_measure_time > 0)
51 : {
52 : // std::find_if(objects.begin(), objects.end(), [](const auto& o){return
53 : // !o.is_deleted );
54 :
55 10 : for (Object& current_object : objects)
56 : {
57 6 : if (!current_object.is_deleted)
58 : {
59 : const auto same_object_it{boost::range::find_if(
60 4 : history.back(),
61 12 : [¤t_object](const auto& history_object) {
62 12 : return history_object.id == current_object.id;
63 14 : })};
64 :
65 4 : if (same_object_it != history.back().end())
66 : {
67 : // gets the miliseconds since epoch
68 4 : double time_difference_ms =
69 4 : static_cast<double>(milliseconds_since_epoch)
70 4 : - static_cast<double>(last_measure_time);
71 4 : double time_difference =
72 : time_difference_ms / static_cast<double>(1000);
73 :
74 4 : double x_difference = current_object.middlepoint.x()
75 4 : - same_object_it->middlepoint.x();
76 4 : double y_difference = current_object.middlepoint.y()
77 4 : - same_object_it->middlepoint.y();
78 4 : double z_difference = current_object.middlepoint.z()
79 4 : - same_object_it->middlepoint.z();
80 4 : current_object.speed =
81 8 : Object::Vector(x_difference / time_difference,
82 8 : y_difference / time_difference,
83 12 : z_difference / time_difference);
84 : }
85 : }
86 : }
87 : }
88 4 : last_measure_time = milliseconds_since_epoch;
89 4 : return objects;
90 : }
91 :
92 2 : std::vector<Object> ObjectInterpreter::get_current_objects(
93 : const HLR::Vision::CupDetection& detected_cups,
94 : const pcl::PointCloud<pcl::PointXYZ>& point_cloud)
95 : {
96 2 : merge_objects(detected_cups, point_cloud);
97 2 : return current_objects;
98 : }
99 :
100 7 : std::vector<Object> ObjectInterpreter::get_changed_objects(
101 : const HLR::Vision::CupDetection& detected_cups,
102 : const pcl::PointCloud<pcl::PointXYZ>& point_cloud)
103 : {
104 7 : merge_objects(detected_cups, point_cloud);
105 7 : std::vector<Object> changed_objects;
106 :
107 7 : for (const auto& i : current_objects)
108 : {
109 0 : add_only_changed_objects(i, changed_objects);
110 : }
111 :
112 7 : return changed_objects;
113 : }
114 :
115 2 : void ObjectInterpreter::add_only_changed_objects(
116 : const Object& object,
117 : std::vector<Object>& changed_objects) const
118 : {
119 2 : bool is_new = true;
120 6 : for (const auto& i : history.front())
121 : {
122 4 : if (object.id == i.id)
123 : {
124 2 : is_new = false;
125 2 : if (object != i)
126 : {
127 2 : changed_objects.push_back(object);
128 : }
129 : }
130 : }
131 2 : if (is_new)
132 : {
133 1 : changed_objects.push_back(object);
134 : }
135 2 : }
136 :
137 9 : void ObjectInterpreter::merge_objects(
138 : const HLR::Vision::CupDetection& detected_cups,
139 : const pcl::PointCloud<pcl::PointXYZ>& point_cloud)
140 : {
141 9 : history.push_front(std::move(current_objects));
142 9 : current_objects = lidar_interpreter.get_objects(detected_cups, point_cloud);
143 9 : }
144 :
145 0 : std::vector<Object> ObjectInterpreter::identify_objects(
146 : std::vector<Object> objects)
147 : {
148 0 : std::vector<Object> old_objects = history[0];
149 : // decide the radius to check for objects
150 0 : double radius_object_checking = 0.1; // in meters
151 :
152 : // if all the objects are gone
153 0 : if (objects.empty())
154 : {
155 : // mark all the previous detected object as deleted
156 0 : for (auto& an_old_object : old_objects)
157 : {
158 0 : if (an_old_object.is_deleted == false)
159 : {
160 0 : an_old_object.is_deleted = true;
161 0 : objects.push_back(an_old_object);
162 : }
163 : }
164 0 : return objects;
165 : }
166 : else
167 : {
168 : // create changed object list
169 0 : for (auto& an_object : objects)
170 : {
171 : // gather objects within a radius of this object
172 0 : std::vector<Object> objects_within_range;
173 0 : for (auto& an_old_object : old_objects)
174 : {
175 : bool is_within_radius =
176 0 : std::fabs(an_old_object.middlepoint.x())
177 : <= radius_object_checking
178 0 : && std::fabs(an_old_object.middlepoint.y())
179 : <= radius_object_checking
180 0 : && std::fabs(an_old_object.middlepoint.z())
181 0 : <= radius_object_checking;
182 0 : if (is_within_radius)
183 : {
184 0 : objects_within_range.push_back(an_old_object);
185 : }
186 : }
187 : // if no objects
188 0 : if (objects_within_range.size() == 0)
189 : {
190 : // do nothing because the object is apperently new.
191 : }
192 : // if one or more objects
193 : else
194 : {
195 0 : std::optional<double> MADD = std::nullopt;
196 0 : Object MADDest_object;
197 : // calculate the permutations
198 0 : for (auto& object_within_range : objects_within_range)
199 : {
200 : // calculate distance from old object within range to
201 : // current object
202 : double distance_current_object =
203 0 : std::sqrt(std::pow(an_object.middlepoint.x(), 2)
204 0 : + std::pow(an_object.middlepoint.y(), 2));
205 0 : double distance_object_within_range = std::sqrt(
206 0 : std::pow(object_within_range.middlepoint.x(), 2)
207 0 : + std::pow(object_within_range.middlepoint.y(), 2));
208 : // calculate highest maximum obsolute distance decrease
209 : // (MADD)
210 0 : double distance_delta =
211 0 : distance_current_object - distance_object_within_range;
212 : // decide the object based on MADD
213 0 : if (MADD || MADD < distance_delta)
214 : {
215 0 : MADD = distance_delta;
216 : // copy info over from the highest MADD object to the
217 : // current object
218 0 : an_object.id = object_within_range.id;
219 0 : an_object.speed = object_within_range.speed;
220 0 : an_object.polyhedron = object_within_range.polyhedron;
221 0 : an_object.is_cup = object_within_range.is_cup;
222 : }
223 : }
224 : // mark old and unused objects as deleted
225 0 : for (auto& object_within_range : objects_within_range)
226 : {
227 0 : if (!object_within_range.is_deleted
228 0 : && object_within_range.id != an_object.id)
229 : {
230 0 : object_within_range.is_deleted = true;
231 0 : objects.push_back(object_within_range);
232 : }
233 : }
234 : }
235 : }
236 0 : return objects;
237 : }
238 : }
239 :
240 3 : std::vector<Object> ObjectInterpreter::isolate_objects(
241 : const pcl::PointCloud<pcl::PointXYZ>& newest_cloud)
242 : {
243 3 : constexpr double height = 3.0;
244 :
245 : // Empty Object vector which is going to contain all objects.
246 : // Will be returned at the end of the function.
247 3 : std::vector<Object> found_objects;
248 :
249 : // Check if input contains data to prevent program from crashing.
250 3 : if (!newest_cloud.empty())
251 : {
252 : // This function needs a boost shared_pointer to function.
253 : auto cloud_pointer =
254 : boost::make_shared<const pcl::PointCloud<pcl::PointXYZ>>(
255 6 : newest_cloud);
256 : pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_pointer(
257 6 : new pcl::PointCloud<pcl::PointXYZ>);
258 :
259 : // Variables and setup for search algorithm for Euclidian clustering.
260 : pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(
261 6 : new pcl::search::KdTree<pcl::PointXYZ>);
262 3 : kdtree->setInputCloud(cloud_pointer);
263 :
264 : // Euclidean clustering object.
265 6 : pcl::EuclideanClusterExtraction<pcl::PointXYZ> cluster_object;
266 :
267 : // Set cluster tolerance to 5cm (small values may cause objects to be
268 : // divided in several clusters, whereas big values may join objects in a
269 : // same cluster).
270 3 : cluster_object.setClusterTolerance(0.05);
271 :
272 : // Set the minimum and maximum number of points that a cluster can have.
273 3 : cluster_object.setMinClusterSize(2);
274 3 : cluster_object.setMaxClusterSize(100);
275 :
276 : // Set the search method for Euclidian clustering.
277 3 : cluster_object.setSearchMethod(kdtree);
278 :
279 : // Set input for Euclidian clustering.
280 3 : cluster_object.setInputCloud(cloud_pointer);
281 :
282 : // Output vector for detected clusters.
283 6 : std::vector<pcl::PointIndices> clusters;
284 :
285 : // Function to extract clusters from input cloud.
286 3 : cluster_object.extract(clusters);
287 :
288 : // Loop through every found object (cluster of points).
289 54 : for (const auto& c : clusters)
290 : {
291 : // Temp PointCloud for adding to an Object later on.
292 102 : pcl::PointCloud<pcl::PointXYZ> temp;
293 :
294 : // Set the rest of the Object and pointcloud parameters.
295 51 : temp.width =
296 51 : static_cast<unsigned int>(cloud_pointer->points.size());
297 51 : temp.height = 0;
298 51 : temp.is_dense = true;
299 :
300 : /* Temporary vector containing polar coordinates for all points in
301 : * the cluster */
302 102 : std::vector<std::pair<double, double>> polar_vector;
303 :
304 : // Loop through every point in the clustered object.
305 303 : for (auto point_signed : c.indices)
306 : {
307 252 : const auto point{std::size_t(point_signed)};
308 : // Put Points in temp and push back x and y to temporary vectors
309 : // for calculating average
310 252 : temp.points.push_back(cloud_pointer->points[point]);
311 252 : polar_vector.emplace_back(
312 504 : std::sqrt(
313 252 : double(std::pow(cloud_pointer->points[point].x, 2))
314 252 : + std::pow(cloud_pointer->points[point].y, 2)),
315 504 : std::atan2(double(cloud_pointer->points[point].y),
316 252 : cloud_pointer->points[point].x));
317 : }
318 :
319 : // Sort temporary vector from low to high
320 51 : boost::range::sort(polar_vector,
321 448 : [](const auto& lhs, const auto& rhs) {
322 448 : return lhs.second < rhs.second;
323 448 : });
324 :
325 : // Calculate distance between the two point with the greatest
326 : // distance in between
327 : double max_distance;
328 : {
329 51 : std::pair<double, double> min_point, max_point;
330 102 : if ((polar_vector.back().second - polar_vector.front().second)
331 51 : <= M_PI)
332 : {
333 48 : min_point = polar_vector.front();
334 48 : max_point = polar_vector.back();
335 : }
336 : else
337 : {
338 : const auto* first_positive_element_iter{
339 6 : &*boost::range::find_if(
340 : polar_vector,
341 27 : [](const auto& e) { return e.second > 0; })};
342 3 : max_point = *first_positive_element_iter;
343 3 : min_point = *--first_positive_element_iter;
344 : }
345 51 : max_distance = std::sqrt(
346 51 : std::pow(max_point.first, 2) + std::pow(min_point.first, 2)
347 51 : - 2 * min_point.first * max_point.first
348 51 : * cos(max_point.second - min_point.second));
349 : }
350 :
351 : // Duplicate points and place them near original points, this is
352 : // needed for 3D object
353 51 : polar_vector.reserve(polar_vector.size() * 2);
354 51 : boost::range::transform(
355 : polar_vector,
356 : std::back_inserter(polar_vector),
357 504 : [max_distance](const auto& e) -> std::pair<double, double> {
358 504 : return {e.first + max_distance, e.second};
359 51 : });
360 :
361 : // Add height to object and create Polyhedron
362 : using Point2 = CGAL::Simple_cartesian<double>::Point_2;
363 : using Point3 = CGAL::Simple_cartesian<double>::Point_3;
364 102 : std::vector<Point2> front_and_back_points;
365 51 : boost::range::transform(polar_vector,
366 : std::back_inserter(front_and_back_points),
367 504 : [](const auto& e) {
368 : return Point2(
369 1008 : e.first * std::cos(e.second),
370 1008 : e.first * std::sin(e.second));
371 1059 : });
372 102 : std::vector<Point2> extreme_points;
373 51 : CGAL::convex_hull_2(front_and_back_points.begin(),
374 : front_and_back_points.end(),
375 51 : std::back_inserter(extreme_points));
376 :
377 : using Polyhedron = Object::Polyhedron;
378 102 : Polyhedron polyhedron;
379 102 : std::vector<Polyhedron::Vertex_index> upper_rim, lower_rim;
380 51 : boost::range::transform(
381 : extreme_points,
382 : std::back_inserter(upper_rim),
383 298 : [&polyhedron](const auto& p) {
384 596 : return polyhedron.add_vertex(
385 596 : Point3(p.y() / 1000.0, height / 2, p.x() / 1000.0));
386 647 : });
387 51 : boost::range::transform(
388 : extreme_points,
389 : std::back_inserter(lower_rim),
390 298 : [&polyhedron](const auto& p) {
391 596 : return polyhedron.add_vertex(
392 596 : Point3(p.y() / 1000.0, -height / 2, p.x() / 1000.0));
393 647 : });
394 :
395 : // Add 3D points to Polyhedron
396 647 : for (std::size_t i = 0, j = i + 1; i < extreme_points.size();
397 298 : ++i, j = (i + 1) % extreme_points.size())
398 : {
399 298 : polyhedron.add_face(upper_rim[i], upper_rim[j], lower_rim[i]);
400 298 : polyhedron.add_face(upper_rim[j], lower_rim[j], lower_rim[i]);
401 : }
402 :
403 : // Set the rest of the Object and pointcloud parameters.
404 51 : temp.width =
405 51 : static_cast<unsigned int>(cloud_pointer->points.size());
406 51 : temp.height = 0;
407 51 : temp.is_dense = true;
408 :
409 102 : Object temp_object;
410 51 : temp_object.id = 0;
411 51 : temp_object.polyhedron = polyhedron;
412 51 : temp_object.speed = std::nullopt;
413 51 : temp_object.is_cup = false;
414 51 : temp_object.is_deleted = false;
415 :
416 : // Calculate northest, southest, most west and most east lying point
417 : // for calculating average
418 : {
419 51 : decltype(extreme_points)::iterator north, south, west, east;
420 51 : CGAL::ch_nswe_point(extreme_points.begin(),
421 : extreme_points.end(),
422 : north,
423 : south,
424 : west,
425 : east);
426 :
427 : // Calculate middlepoint by taking average.
428 51 : temp_object.middlepoint =
429 102 : Point3((north->y() + south->y()) / 2000.0,
430 102 : static_cast<double>(0),
431 102 : (east->x() + west->x()) / 2000.0);
432 : }
433 51 : temp_object.point_cloud = temp;
434 :
435 : // Push information back to pointcloud to return.
436 51 : found_objects.push_back(temp_object);
437 : }
438 : }
439 : // Return found objects.
440 3 : return found_objects;
441 : }
442 :
443 : } // namespace PointCloud
444 3 : } // namespace HLR
|
