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