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 3 : std::vector<Object> ObjectInterpreter::isolate_objects(
146 : const pcl::PointCloud<pcl::PointXYZ>& newest_cloud)
147 : {
148 3 : constexpr double height = 3.0;
149 :
150 : // Empty Object vector which is going to contain all objects.
151 : // Will be returned at the end of the function.
152 3 : std::vector<Object> found_objects;
153 :
154 : // Check if input contains data to prevent program from crashing.
155 3 : if (!newest_cloud.empty())
156 : {
157 : // This function needs a boost shared_pointer to function.
158 : auto cloud_pointer =
159 : boost::make_shared<const pcl::PointCloud<pcl::PointXYZ>>(
160 6 : newest_cloud);
161 : pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_pointer(
162 6 : new pcl::PointCloud<pcl::PointXYZ>);
163 :
164 : // Variables and setup for search algorithm for Euclidian clustering.
165 : pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(
166 6 : new pcl::search::KdTree<pcl::PointXYZ>);
167 3 : kdtree->setInputCloud(cloud_pointer);
168 :
169 : // Euclidean clustering object.
170 6 : pcl::EuclideanClusterExtraction<pcl::PointXYZ> cluster_object;
171 :
172 : // Set cluster tolerance to 5cm (small values may cause objects to be
173 : // divided in several clusters, whereas big values may join objects in a
174 : // same cluster).
175 3 : cluster_object.setClusterTolerance(0.05);
176 :
177 : // Set the minimum and maximum number of points that a cluster can have.
178 3 : cluster_object.setMinClusterSize(2);
179 3 : cluster_object.setMaxClusterSize(100);
180 :
181 : // Set the search method for Euclidian clustering.
182 3 : cluster_object.setSearchMethod(kdtree);
183 :
184 : // Set input for Euclidian clustering.
185 3 : cluster_object.setInputCloud(cloud_pointer);
186 :
187 : // Output vector for detected clusters.
188 6 : std::vector<pcl::PointIndices> clusters;
189 :
190 : // Function to extract clusters from input cloud.
191 3 : cluster_object.extract(clusters);
192 :
193 : // Loop through every found object (cluster of points).
194 54 : for (const auto& c : clusters)
195 : {
196 : // Temp PointCloud for adding to an Object later on.
197 102 : pcl::PointCloud<pcl::PointXYZ> temp;
198 :
199 : // Set the rest of the Object and pointcloud parameters.
200 51 : temp.width =
201 51 : static_cast<unsigned int>(cloud_pointer->points.size());
202 51 : temp.height = 0;
203 51 : temp.is_dense = true;
204 :
205 : /* Temporary vector containing polar coordinates for all points in
206 : * the cluster */
207 102 : std::vector<std::pair<double, double>> polar_vector;
208 :
209 : // Loop through every point in the clustered object.
210 303 : for (auto point_signed : c.indices)
211 : {
212 252 : const auto point{std::size_t(point_signed)};
213 : // Put Points in temp and push back x and y to temporary vectors
214 : // for calculating average
215 252 : temp.points.push_back(cloud_pointer->points[point]);
216 252 : polar_vector.emplace_back(
217 504 : std::sqrt(
218 252 : double(std::pow(cloud_pointer->points[point].x, 2))
219 252 : + std::pow(cloud_pointer->points[point].y, 2)),
220 504 : std::atan2(double(cloud_pointer->points[point].y),
221 252 : cloud_pointer->points[point].x));
222 : }
223 :
224 : // Sort temporary vector from low to high
225 51 : boost::range::sort(polar_vector,
226 448 : [](const auto& lhs, const auto& rhs) {
227 448 : return lhs.second < rhs.second;
228 448 : });
229 :
230 : // Calculate distance between the two point with the greatest
231 : // distance in between
232 : double max_distance;
233 : {
234 51 : std::pair<double, double> min_point, max_point;
235 102 : if ((polar_vector.back().second - polar_vector.front().second)
236 51 : <= M_PI)
237 : {
238 48 : min_point = polar_vector.front();
239 48 : max_point = polar_vector.back();
240 : }
241 : else
242 : {
243 : const auto* first_positive_element_iter{
244 6 : &*boost::range::find_if(
245 : polar_vector,
246 27 : [](const auto& e) { return e.second > 0; })};
247 3 : max_point = *first_positive_element_iter;
248 3 : min_point = *--first_positive_element_iter;
249 : }
250 51 : max_distance = std::sqrt(
251 51 : std::pow(max_point.first, 2) + std::pow(min_point.first, 2)
252 51 : - 2 * min_point.first * max_point.first
253 51 : * cos(max_point.second - min_point.second));
254 : }
255 :
256 : // Duplicate points and place them near original points, this is
257 : // needed for 3D object
258 51 : polar_vector.reserve(polar_vector.size() * 2);
259 51 : boost::range::transform(
260 : polar_vector,
261 : std::back_inserter(polar_vector),
262 504 : [max_distance](const auto& e) -> std::pair<double, double> {
263 504 : return {e.first + max_distance, e.second};
264 51 : });
265 :
266 : // Add height to object and create Polyhedron
267 : using Point2 = CGAL::Simple_cartesian<double>::Point_2;
268 : using Point3 = CGAL::Simple_cartesian<double>::Point_3;
269 102 : std::vector<Point2> front_and_back_points;
270 51 : boost::range::transform(polar_vector,
271 : std::back_inserter(front_and_back_points),
272 504 : [](const auto& e) {
273 : return Point2(
274 1008 : e.first * std::cos(e.second),
275 1008 : e.first * std::sin(e.second));
276 1059 : });
277 102 : std::vector<Point2> extreme_points;
278 51 : CGAL::convex_hull_2(front_and_back_points.begin(),
279 : front_and_back_points.end(),
280 51 : std::back_inserter(extreme_points));
281 :
282 : using Polyhedron = Object::Polyhedron;
283 102 : Polyhedron polyhedron;
284 102 : std::vector<Polyhedron::Vertex_index> upper_rim, lower_rim;
285 51 : boost::range::transform(
286 : extreme_points,
287 : std::back_inserter(upper_rim),
288 298 : [&polyhedron](const auto& p) {
289 596 : return polyhedron.add_vertex(
290 596 : Point3(p.y() / 1000.0, height / 2, p.x() / 1000.0));
291 647 : });
292 51 : boost::range::transform(
293 : extreme_points,
294 : std::back_inserter(lower_rim),
295 298 : [&polyhedron](const auto& p) {
296 596 : return polyhedron.add_vertex(
297 596 : Point3(p.y() / 1000.0, -height / 2, p.x() / 1000.0));
298 647 : });
299 :
300 : // Add 3D points to Polyhedron
301 647 : for (std::size_t i = 0, j = i + 1; i < extreme_points.size();
302 298 : ++i, j = (i + 1) % extreme_points.size())
303 : {
304 298 : polyhedron.add_face(upper_rim[i], upper_rim[j], lower_rim[i]);
305 298 : polyhedron.add_face(upper_rim[j], lower_rim[j], lower_rim[i]);
306 : }
307 :
308 : // Set the rest of the Object and pointcloud parameters.
309 51 : temp.width =
310 51 : static_cast<unsigned int>(cloud_pointer->points.size());
311 51 : temp.height = 0;
312 51 : temp.is_dense = true;
313 :
314 102 : Object temp_object;
315 51 : temp_object.id = 0;
316 51 : temp_object.polyhedron = polyhedron;
317 51 : temp_object.speed = std::nullopt;
318 51 : temp_object.is_cup = false;
319 51 : temp_object.is_deleted = false;
320 :
321 : // Calculate northest, southest, most west and most east lying point
322 : // for calculating average
323 : {
324 51 : decltype(extreme_points)::iterator north, south, west, east;
325 51 : CGAL::ch_nswe_point(extreme_points.begin(),
326 : extreme_points.end(),
327 : north,
328 : south,
329 : west,
330 : east);
331 :
332 : // Calculate middlepoint by taking average.
333 51 : temp_object.middlepoint =
334 102 : Point3((north->y() + south->y()) / 2000.0,
335 102 : static_cast<double>(0),
336 102 : (east->x() + west->x()) / 2000.0);
337 : }
338 51 : temp_object.point_cloud = temp;
339 :
340 : // Push information back to pointcloud to return.
341 51 : found_objects.push_back(temp_object);
342 : }
343 : }
344 : // Return found objects.
345 3 : return found_objects;
346 : }
347 :
348 : } // namespace PointCloud
349 3 : } // namespace HLR
|
