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 2 : std::vector<Object> ObjectInterpreter::get_current_objects(
26 : const HLR::Vision::CupDetection& detected_cups,
27 : const pcl::PointCloud<pcl::PointXYZ>& point_cloud)
28 : {
29 2 : merge_objects(detected_cups, point_cloud);
30 2 : return current_objects;
31 : }
32 :
33 7 : std::vector<Object> ObjectInterpreter::get_changed_objects(
34 : const HLR::Vision::CupDetection& detected_cups,
35 : const pcl::PointCloud<pcl::PointXYZ>& point_cloud)
36 : {
37 7 : merge_objects(detected_cups, point_cloud);
38 7 : std::vector<Object> changed_objects;
39 :
40 7 : for (const auto& i : current_objects)
41 : {
42 0 : add_only_changed_objects(i, changed_objects);
43 : }
44 :
45 7 : return changed_objects;
46 : }
47 :
48 2 : void ObjectInterpreter::add_only_changed_objects(
49 : const Object& object,
50 : std::vector<Object>& changed_objects) const
51 : {
52 2 : bool is_new = true;
53 6 : for (const auto& i : history.front())
54 : {
55 4 : if (object.id == i.id)
56 : {
57 2 : is_new = false;
58 2 : if (object != i)
59 : {
60 2 : changed_objects.push_back(object);
61 : }
62 : }
63 : }
64 2 : if (is_new)
65 : {
66 1 : changed_objects.push_back(object);
67 : }
68 2 : }
69 :
70 9 : void ObjectInterpreter::merge_objects(
71 : const HLR::Vision::CupDetection& detected_cups,
72 : const pcl::PointCloud<pcl::PointXYZ>& point_cloud)
73 : {
74 9 : history.push_front(std::move(current_objects));
75 9 : current_objects = lidar_interpreter.get_objects(detected_cups, point_cloud);
76 9 : }
77 :
78 3 : std::vector<Object> ObjectInterpreter::isolate_objects(
79 : const pcl::PointCloud<pcl::PointXYZ>& newest_cloud)
80 : {
81 3 : constexpr double height = 3.0;
82 :
83 : // Empty Object vector which is going to contain all objects.
84 : // Will be returned at the end of the function.
85 3 : std::vector<Object> found_objects;
86 :
87 : // Check if input contains data to prevent program from crashing.
88 3 : if (!newest_cloud.empty())
89 : {
90 : // This function needs a boost shared_pointer to function.
91 : auto cloud_pointer =
92 : boost::make_shared<const pcl::PointCloud<pcl::PointXYZ>>(
93 6 : newest_cloud);
94 : pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_pointer(
95 6 : new pcl::PointCloud<pcl::PointXYZ>);
96 :
97 : // Variables and setup for search algorithm for Euclidian clustering.
98 : pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(
99 6 : new pcl::search::KdTree<pcl::PointXYZ>);
100 3 : kdtree->setInputCloud(cloud_pointer);
101 :
102 : // Euclidean clustering object.
103 6 : pcl::EuclideanClusterExtraction<pcl::PointXYZ> cluster_object;
104 :
105 : // Set cluster tolerance to 5cm (small values may cause objects to be
106 : // divided in several clusters, whereas big values may join objects in a
107 : // same cluster).
108 3 : cluster_object.setClusterTolerance(0.05);
109 :
110 : // Set the minimum and maximum number of points that a cluster can have.
111 3 : cluster_object.setMinClusterSize(2);
112 3 : cluster_object.setMaxClusterSize(100);
113 :
114 : // Set the search method for Euclidian clustering.
115 3 : cluster_object.setSearchMethod(kdtree);
116 :
117 : // Set input for Euclidian clustering.
118 3 : cluster_object.setInputCloud(cloud_pointer);
119 :
120 : // Output vector for detected clusters.
121 6 : std::vector<pcl::PointIndices> clusters;
122 :
123 : // Function to extract clusters from input cloud.
124 3 : cluster_object.extract(clusters);
125 :
126 : // Loop through every found object (cluster of points).
127 54 : for (const auto& c : clusters)
128 : {
129 : // Temp PointCloud for adding to an Object later on.
130 102 : pcl::PointCloud<pcl::PointXYZ> temp;
131 :
132 : // Set the rest of the Object and pointcloud parameters.
133 51 : temp.width =
134 51 : static_cast<unsigned int>(cloud_pointer->points.size());
135 51 : temp.height = 0;
136 51 : temp.is_dense = true;
137 :
138 : /* Temporary vector containing polar coordinates for all points in
139 : * the cluster */
140 102 : std::vector<std::pair<double, double>> polar_vector;
141 :
142 : // Loop through every point in the clustered object.
143 303 : for (auto point_signed : c.indices)
144 : {
145 252 : const auto point{std::size_t(point_signed)};
146 : // Put Points in temp and push back x and y to temporary vectors
147 : // for calculating average
148 252 : temp.points.push_back(cloud_pointer->points[point]);
149 252 : polar_vector.emplace_back(
150 504 : std::sqrt(
151 252 : double(std::pow(cloud_pointer->points[point].x, 2))
152 252 : + std::pow(cloud_pointer->points[point].y, 2)),
153 504 : std::atan2(double(cloud_pointer->points[point].y),
154 252 : cloud_pointer->points[point].x));
155 : }
156 :
157 : // Sort temporary vector from low to high
158 51 : boost::range::sort(polar_vector,
159 448 : [](const auto& lhs, const auto& rhs) {
160 448 : return lhs.second < rhs.second;
161 448 : });
162 :
163 : // Calculate distance between the two point with the greatest
164 : // distance in between
165 : double max_distance;
166 : {
167 51 : std::pair<double, double> min_point, max_point;
168 102 : if ((polar_vector.back().second - polar_vector.front().second)
169 51 : <= M_PI)
170 : {
171 48 : min_point = polar_vector.front();
172 48 : max_point = polar_vector.back();
173 : }
174 : else
175 : {
176 : const auto* first_positive_element_iter{
177 6 : &*boost::range::find_if(
178 : polar_vector,
179 27 : [](const auto& e) { return e.second > 0; })};
180 3 : max_point = *first_positive_element_iter;
181 3 : min_point = *--first_positive_element_iter;
182 : }
183 51 : max_distance = std::sqrt(
184 51 : std::pow(max_point.first, 2) + std::pow(min_point.first, 2)
185 51 : - 2 * min_point.first * max_point.first
186 51 : * cos(max_point.second - min_point.second));
187 : }
188 :
189 : // Duplicate points and place them near original points, this is
190 : // needed for 3D object
191 51 : polar_vector.reserve(polar_vector.size() * 2);
192 51 : boost::range::transform(
193 : polar_vector,
194 : std::back_inserter(polar_vector),
195 504 : [max_distance](const auto& e) -> std::pair<double, double> {
196 504 : return {e.first + max_distance, e.second};
197 51 : });
198 :
199 : // Add height to object and create Polyhedron
200 : using Point2 = CGAL::Simple_cartesian<double>::Point_2;
201 : using Point3 = CGAL::Simple_cartesian<double>::Point_3;
202 102 : std::vector<Point2> front_and_back_points;
203 51 : boost::range::transform(polar_vector,
204 : std::back_inserter(front_and_back_points),
205 504 : [](const auto& e) {
206 : return Point2(
207 1008 : e.first * std::cos(e.second),
208 1008 : e.first * std::sin(e.second));
209 1059 : });
210 102 : std::vector<Point2> extreme_points;
211 51 : CGAL::convex_hull_2(front_and_back_points.begin(),
212 : front_and_back_points.end(),
213 51 : std::back_inserter(extreme_points));
214 :
215 : using Polyhedron = Object::Polyhedron;
216 102 : Polyhedron polyhedron;
217 102 : std::vector<Polyhedron::Vertex_index> upper_rim, lower_rim;
218 51 : boost::range::transform(
219 : extreme_points,
220 : std::back_inserter(upper_rim),
221 298 : [&polyhedron](const auto& p) {
222 596 : return polyhedron.add_vertex(
223 596 : Point3(p.y() / 1000.0, height / 2, p.x() / 1000.0));
224 647 : });
225 51 : boost::range::transform(
226 : extreme_points,
227 : std::back_inserter(lower_rim),
228 298 : [&polyhedron](const auto& p) {
229 596 : return polyhedron.add_vertex(
230 596 : Point3(p.y() / 1000.0, -height / 2, p.x() / 1000.0));
231 647 : });
232 :
233 : // Add 3D points to Polyhedron
234 647 : for (std::size_t i = 0, j = i + 1; i < extreme_points.size();
235 298 : ++i, j = (i + 1) % extreme_points.size())
236 : {
237 298 : polyhedron.add_face(upper_rim[i], upper_rim[j], lower_rim[i]);
238 298 : polyhedron.add_face(upper_rim[j], lower_rim[j], lower_rim[i]);
239 : }
240 :
241 : // Set the rest of the Object and pointcloud parameters.
242 51 : temp.width =
243 51 : static_cast<unsigned int>(cloud_pointer->points.size());
244 51 : temp.height = 0;
245 51 : temp.is_dense = true;
246 :
247 102 : Object temp_object;
248 51 : temp_object.id = 0;
249 51 : temp_object.polyhedron = polyhedron;
250 51 : temp_object.speed = std::nullopt;
251 51 : temp_object.is_cup = false;
252 51 : temp_object.is_deleted = false;
253 :
254 : // Calculate northest, southest, most west and most east lying point
255 : // for calculating average
256 : {
257 51 : decltype(extreme_points)::iterator north, south, west, east;
258 51 : CGAL::ch_nswe_point(extreme_points.begin(),
259 : extreme_points.end(),
260 : north,
261 : south,
262 : west,
263 : east);
264 :
265 : // Calculate middlepoint by taking average.
266 51 : temp_object.middlepoint =
267 102 : Point3((north->y() + south->y()) / 2000.0,
268 102 : static_cast<double>(0),
269 102 : (east->x() + west->x()) / 2000.0);
270 : }
271 51 : temp_object.point_cloud = temp;
272 :
273 : // Push information back to pointcloud to return.
274 51 : found_objects.push_back(temp_object);
275 : }
276 : }
277 : // Return found objects.
278 3 : return found_objects;
279 : }
280 :
281 : } // namespace PointCloud
282 3 : } // namespace HLR
|
