Line data Source code
1 : #include "LidarInterpreter.hpp"
2 : #include <cmath>
3 : #include <boost/make_shared.hpp>
4 : #include <pcl/PointIndices.h>
5 : #include <pcl/io/pcd_io.h>
6 : #include <pcl/octree/octree.h>
7 : #include <pcl/octree/octree_impl.h>
8 : #include <pcl/octree/octree_search.h>
9 : #include <pcl/point_types.h>
10 : #include <pcl/search/kdtree.h>
11 : #include <pcl/segmentation/extract_clusters.h>
12 :
13 : namespace HLR
14 : {
15 : namespace PointCloud
16 : {
17 1 : const LidarInterpreter::CupMeasurements LidarInterpreter::measured_cup_values =
18 : CupMeasurements{0.03, 0.0225, 0.079}; // meters
19 : // Constant translation from LiDAR's coordinate system to robot arm's
20 : const CGAL::Simple_cartesian<double>::Vector_3
21 1 : LidarInterpreter::lidar_to_robot_arm_translation{1.5, 0, 2};
22 :
23 10 : std::vector<Object> LidarInterpreter::get_objects(
24 : const HLR::Vision::CupDetection& detected_cups,
25 : const pcl::PointCloud<pcl::PointXYZ>& point_cloud)
26 : {
27 10 : std::vector<Object> objects;
28 11 : for (const auto& cup : detected_cups.cups)
29 : {
30 1 : objects.push_back(convert_cup_to_polyhedron(
31 2 : translate_lidar_to_robot_arm(
32 2 : get_centerpoint_from_contour(find_contour(
33 : translate_kinect_to_lidar(detected_cups.frame, cup),
34 : point_cloud))),
35 : point_cloud));
36 : }
37 10 : return objects;
38 : }
39 :
40 24 : Object LidarInterpreter::convert_cup_to_polyhedron(
41 : const LidarInterpreter::Cup& cup_in_robot_arm_space,
42 : const pcl::PointCloud<pcl::PointXYZ>& point_cloud)
43 : {
44 : using Point = Object::Point;
45 : using Vector = CGAL::Simple_cartesian<double>::Vector_3;
46 :
47 : // We need an even number of faces to ease generation of rim points.
48 : static_assert(number_of_cylinder_faces % 2 == 0);
49 :
50 24 : Object object{cup_in_robot_arm_space.id,
51 : {},
52 : std::nullopt,
53 : true,
54 : false,
55 : point_cloud,
56 24 : cup_in_robot_arm_space.centerpoint};
57 24 : Object::Polyhedron& polyhedron = object.polyhedron;
58 :
59 24 : const Vector distance_center_top{0, cup_in_robot_arm_space.height / 2, 0};
60 : const Point upper_center =
61 24 : cup_in_robot_arm_space.centerpoint + distance_center_top;
62 : const Point lower_center =
63 24 : cup_in_robot_arm_space.centerpoint - distance_center_top;
64 : const Object::Polyhedron::vertex_index upper_center_vertex =
65 24 : polyhedron.add_vertex(upper_center);
66 : const Object::Polyhedron::vertex_index lower_center_vertex =
67 24 : polyhedron.add_vertex(lower_center);
68 :
69 : std::array<Object::Polyhedron::vertex_index, number_of_cylinder_faces>
70 24 : upper_vertices;
71 : std::array<Object::Polyhedron::vertex_index, number_of_cylinder_faces>
72 24 : lower_vertices;
73 312 : for (unsigned i = 0; i < number_of_cylinder_faces / 2; ++i)
74 : {
75 : const Vector unit_translation = {
76 576 : sin(2 * i * M_PI / number_of_cylinder_faces),
77 : 0,
78 288 : cos(2 * i * M_PI / number_of_cylinder_faces)};
79 :
80 288 : upper_vertices[i] = polyhedron.add_vertex(
81 : upper_center
82 576 : + unit_translation * cup_in_robot_arm_space.upper_radius);
83 288 : lower_vertices[i] = polyhedron.add_vertex(
84 : lower_center
85 576 : + unit_translation * cup_in_robot_arm_space.lower_radius);
86 288 : upper_vertices[number_of_cylinder_faces / 2 + i] =
87 : polyhedron.add_vertex(upper_center
88 576 : - unit_translation
89 864 : * cup_in_robot_arm_space.upper_radius);
90 288 : lower_vertices[number_of_cylinder_faces / 2 + i] =
91 : polyhedron.add_vertex(lower_center
92 576 : - unit_translation
93 864 : * cup_in_robot_arm_space.lower_radius);
94 : }
95 :
96 1176 : for (unsigned i = 0, j = i + 1; i < number_of_cylinder_faces;
97 576 : ++i, j = (i + 1) % number_of_cylinder_faces)
98 : {
99 576 : polyhedron.add_face(
100 576 : upper_vertices[i], upper_center_vertex, upper_vertices[j]);
101 576 : polyhedron.add_face(
102 576 : lower_vertices[i], lower_vertices[j], lower_center_vertex);
103 :
104 576 : polyhedron.add_face(
105 576 : upper_vertices[i], upper_vertices[j], lower_vertices[j]);
106 576 : polyhedron.add_face(
107 576 : upper_vertices[i], lower_vertices[j], lower_vertices[i]);
108 : }
109 :
110 24 : return object;
111 : }
112 :
113 2 : LidarInterpreter::Cup LidarInterpreter::translate_lidar_to_robot_arm(
114 : const LidarInterpreter::Cup& cup_in_lidar_space)
115 : {
116 2 : Cup robot_arm_cup{cup_in_lidar_space};
117 2 : robot_arm_cup.centerpoint =
118 : robot_arm_cup.centerpoint + lidar_to_robot_arm_translation;
119 2 : return robot_arm_cup;
120 : }
121 :
122 2 : LidarInterpreter::Cup LidarInterpreter::get_centerpoint_from_contour(
123 : const LidarInterpreter::PointCloudWithId& point_cloud_with_id)
124 : {
125 : // left hand cartesian 2D points in meters
126 : const pcl::PointCloud<pcl::PointXYZ> point_cloud =
127 4 : point_cloud_with_id.first;
128 : using PolarPoint = std::pair<double, double>;
129 2 : LidarInterpreter::Cup result;
130 2 : result.upper_radius = measured_cup_values.upper_radius;
131 2 : result.lower_radius = measured_cup_values.lower_radius;
132 2 : result.height = measured_cup_values.height;
133 2 : result.id = point_cloud_with_id.second;
134 :
135 : double distance;
136 : double theta;
137 2 : pcl::PointXYZ nearest_cup_point;
138 2 : const int far_far_away =
139 : 1000; // Anything way further than the nearest point, in meters.
140 2 : PolarPoint centerpoint_polar(0, far_far_away);
141 314162 : for (const auto& point : point_cloud)
142 : {
143 314160 : distance =
144 314160 : std::sqrt(std::pow(point.x, 2) + std::pow(point.y, 2)); // meters
145 314160 : theta = std::acos(point.y / distance); // radians
146 314160 : if (distance < centerpoint_polar.second)
147 : {
148 308318 : centerpoint_polar.first = theta;
149 308318 : centerpoint_polar.second = distance;
150 308318 : nearest_cup_point = point;
151 : }
152 : }
153 :
154 : // add the radius on lidar height to the distance so it becomes the
155 : // centerpoint point.
156 2 : double cup_radius_at_lidar_height =
157 2 : result.lower_radius
158 2 : + (result.upper_radius - result.lower_radius) / result.height
159 2 : * lidar_height;
160 2 : centerpoint_polar.second += cup_radius_at_lidar_height;
161 :
162 : // convert to left hand 3D cartesian in meters
163 2 : result.centerpoint = Object::Point(
164 4 : std::sin(centerpoint_polar.first) * centerpoint_polar.second,
165 4 : result.height / 2,
166 4 : std::cos(centerpoint_polar.first) * centerpoint_polar.second);
167 :
168 4 : return result;
169 : }
170 :
171 2 : LidarInterpreter::PointCloudWithId LidarInterpreter::find_contour(
172 : double angle,
173 : const pcl::PointCloud<pcl::PointXYZ>& newest_cloud)
174 : {
175 : // Empty PointCloud which is going to contain the pointcloud of the found
176 : // cup to return at the end of this function.
177 2 : PointCloudWithId newest_pointcloud;
178 :
179 : // Check if input contains data to prevent program from crashing.
180 2 : if (!newest_cloud.empty())
181 : {
182 : // This function needs a boost shared_pointer to function.
183 : auto cloud_pointer =
184 : boost::make_shared<const pcl::PointCloud<pcl::PointXYZ>>(
185 4 : newest_cloud);
186 : pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_pointer(
187 4 : new pcl::PointCloud<pcl::PointXYZ>);
188 :
189 : // Variables and setup for search algorithm for Euclidian clustering.
190 : pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(
191 4 : new pcl::search::KdTree<pcl::PointXYZ>);
192 2 : kdtree->setInputCloud(cloud_pointer);
193 :
194 : // Euclidean clustering object.
195 4 : pcl::EuclideanClusterExtraction<pcl::PointXYZ> cluster_object;
196 :
197 : // Set cluster tolerance to 2cm (small values may cause objects to be
198 : // divided in several clusters, whereas big values may join objects in a
199 : // same cluster).
200 2 : cluster_object.setClusterTolerance(0.05);
201 :
202 : // Set the minimum and maximum number of points that a cluster can have.
203 2 : cluster_object.setMinClusterSize(2);
204 2 : cluster_object.setMaxClusterSize(100);
205 :
206 : // Set the search method for Euclidian clustering.
207 2 : cluster_object.setSearchMethod(kdtree);
208 :
209 : // Set input for Euclidian clustering.
210 2 : cluster_object.setInputCloud(cloud_pointer);
211 :
212 : // Output vector for detected clusters.
213 4 : std::vector<pcl::PointIndices> clusters;
214 :
215 : // Function to extract clusters from input cloud.
216 2 : cluster_object.extract(clusters);
217 :
218 : // Loop through every found cluster.
219 39 : for (const auto& c : clusters)
220 : {
221 : // Temporary vector for sorting angles to determine if cluster is
222 : // between input angle later on.
223 75 : std::vector<double> min_max_angle;
224 :
225 : // Loop through every point in the cluster.
226 252 : for (const auto& k : c.indices)
227 : {
228 : // Push back x and y of every point in the min_max_angle vector
229 : // so it can be sorted.
230 214 : min_max_angle.push_back(atan2(
231 214 : cloud_pointer->points[static_cast<unsigned long>(k)].y,
232 214 : cloud_pointer->points[static_cast<unsigned long>(k)].x));
233 : }
234 : // Check if min_max_angle contains data.
235 38 : if (!min_max_angle.empty())
236 : {
237 : // Sort angles from low value to high value so the angles can be
238 : // checked later on
239 38 : std::sort(min_max_angle.begin(), min_max_angle.end());
240 :
241 : // Check if angle is bigger than min angle and smaller than max
242 : // angle. If true, the found cluster lies within the search
243 : // angle.
244 76 : if (is_angle_in_angle_range(
245 76 : angle, min_max_angle.front(), min_max_angle.back()))
246 : {
247 : // ++ Pointcloud id counter so each found pointcloud cup
248 : // will be unique.
249 1 : highest_pointcloud_id++;
250 :
251 : // Set current cup id of pointcloud to return.
252 1 : newest_pointcloud.second =
253 1 : static_cast<std::uint32_t>(highest_pointcloud_id);
254 :
255 : // Loop through every point in the cluster again.
256 4 : for (const auto& point : c.indices)
257 : {
258 : // Put Points in pointcloud to return because it's sure
259 : // now this is the right cluster (cup) we are looking
260 : // for.
261 6 : newest_pointcloud.first.points.push_back(
262 : cloud_pointer
263 3 : ->points[static_cast<unsigned long>(point)]);
264 : }
265 :
266 : // Set the rest of the pointcloud parameters.
267 1 : newest_pointcloud.first.width =
268 1 : static_cast<unsigned int>(cloud_pointer->points.size());
269 1 : newest_pointcloud.first.height = 0;
270 1 : newest_pointcloud.first.is_dense = true;
271 :
272 : // Push information back to pointcloud to return.
273 1 : pointcloud_database.push_back(newest_pointcloud);
274 :
275 : // Because the right cluster (cup) is found in the given
276 : // angle break out of loop.
277 1 : break;
278 : }
279 : }
280 : }
281 : }
282 : // Return pointcloud to return.
283 2 : return newest_pointcloud;
284 : }
285 :
286 3 : double LidarInterpreter::translate_kinect_to_lidar(
287 : const std::shared_ptr<cv::Mat>& distance_frame,
288 : const cv::Point& location_cup)
289 : {
290 : // Gathering variables and storing them in an easy to read format.
291 3 : const int x_resolution_img = (*distance_frame).cols;
292 3 : const int x_coord_cup = x_resolution_img / 2 - location_cup.x;
293 3 : const double viewing_angle_kinect = (70.6 / 180.0) * M_PI; // radians
294 : const auto distance_kinect_cup =
295 3 : (*distance_frame).at<float>(location_cup.x, location_cup.y) / 1000.0;
296 :
297 : // Calculating the angle of the Kinect to the cup.
298 3 : const double angle_kinect_cup =
299 6 : std::asin(2 * x_coord_cup
300 3 : * (std::sin(viewing_angle_kinect / 2) / x_resolution_img));
301 :
302 : // Translating the angle of the Kinect to the Lidar.
303 3 : double angle_lidar_cup =
304 6 : std::atan2((distance_kinect_lidar
305 3 : - distance_kinect_cup * std::sin(angle_kinect_cup)),
306 3 : (distance_kinect_cup * std::cos(angle_kinect_cup)));
307 :
308 3 : return angle_lidar_cup;
309 : }
310 :
311 43 : bool LidarInterpreter::is_angle_in_angle_range(double input,
312 : double min_angle,
313 : double max_angle)
314 : {
315 43 : double biggest = 2 * M_PI;
316 43 : if (input < 0)
317 : {
318 13 : input = fmod((2 * M_PI + fmod(input, 2 * M_PI)), 2 * M_PI);
319 : }
320 43 : if (min_angle < 0)
321 : {
322 18 : min_angle = fmod((2 * M_PI + fmod(min_angle, 2 * M_PI)), 2 * M_PI);
323 : }
324 43 : if (max_angle < 0)
325 : {
326 14 : max_angle = fmod((2 * M_PI + fmod(max_angle, 2 * M_PI)), 2 * M_PI);
327 : }
328 :
329 43 : if (input >= min_angle && input >= max_angle)
330 : {
331 2 : biggest = input;
332 : }
333 41 : else if (min_angle >= input && min_angle >= max_angle)
334 : {
335 5 : biggest = min_angle;
336 : }
337 36 : else if (max_angle >= input && max_angle >= min_angle)
338 : {
339 36 : biggest = max_angle;
340 : }
341 :
342 43 : double translation = 2 * M_PI - biggest;
343 43 : input = fmod((translation + input), 2 * M_PI);
344 43 : min_angle = fmod((translation + min_angle), 2 * M_PI);
345 43 : max_angle = fmod((translation + max_angle), 2 * M_PI);
346 :
347 43 : if (min_angle > max_angle)
348 : {
349 36 : std::swap(min_angle, max_angle);
350 : }
351 43 : if (max_angle > M_PI)
352 : {
353 41 : return input >= max_angle;
354 : }
355 2 : return input <= max_angle;
356 : }
357 :
358 : } // namespace PointCloud
359 3 : } // namespace HLR
|
