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