Line data Source code
1 : #include "LidarInterpreter.hpp"
2 :
3 : #include <pcl/io/pcd_io.h>
4 : #include <pcl/point_types.h>
5 : #include <pcl/visualization/cloud_viewer.h>
6 :
7 : #include <pcl/octree/octree.h>
8 : #include <pcl/octree/octree_impl.h>
9 : #include <pcl/octree/octree_search.h>
10 :
11 : #include <pcl/PointIndices.h>
12 : #include <pcl/io/pcd_io.h>
13 : #include <pcl/segmentation/extract_clusters.h>
14 :
15 : #include <boost/make_shared.hpp>
16 :
17 : #include <pcl/search/kdtree.h>
18 :
19 : #include <iostream>
20 :
21 : namespace HLR
22 : {
23 : namespace PointCloud
24 : {
25 : unsigned long LidarInterpreter::highest_pointcloud_id = 0;
26 0 : std::vector<Object> LidarInterpreter::get_objects(
27 : const HLR::Vision::CupDetection& detected_cups,
28 : const pcl::PointCloud<pcl::PointXYZ>& point_cloud)
29 : {
30 0 : std::vector<Object> objects;
31 0 : for (const auto& cup : detected_cups.cups)
32 : {
33 : // objects.push_back(convert_cup_to_polyhedron(translate_lidar_to_robot_arm(get_centerpoint_from_contour(find_contour(translate_kinect_to_lidar(detected_cups.frame,
34 : // cup), point_cloud)))));
35 0 : HLR::Vision::CupDetection tempcup;
36 0 : tempcup.cups.push_back(cup);
37 : }
38 0 : double iets = 1;
39 0 : PointCloudWithId a = find_contour(iets, point_cloud);
40 0 : return objects;
41 : }
42 :
43 0 : Object LidarInterpreter::convert_cup_to_polyhedron(
44 : const LidarInterpreter::Cup& /*cup_in_robot_arm_space*/)
45 : {
46 0 : HLR::PointCloud::Object objects;
47 0 : return objects;
48 : }
49 0 : LidarInterpreter::Cup LidarInterpreter::translate_lidar_to_robot_arm(
50 : const LidarInterpreter::Cup& /*cup_in_lidar_space*/)
51 : {
52 0 : LidarInterpreter::Cup cup;
53 0 : return cup;
54 : }
55 0 : LidarInterpreter::Cup LidarInterpreter::get_centerpoint_from_contour(
56 : const LidarInterpreter::PointCloudWithId& /*point_cloud_with_id*/)
57 : {
58 0 : LidarInterpreter::Cup cup;
59 0 : return cup;
60 : }
61 :
62 2 : LidarInterpreter::PointCloudWithId LidarInterpreter::find_contour(
63 : double angle,
64 : const pcl::PointCloud<pcl::PointXYZ>& newest_cloud)
65 : {
66 : // Empty PointCloud which is going to contain the pointcloud of the found
67 : // cup to return at the end of this function.
68 2 : PointCloudWithId newest_pointcloud;
69 :
70 : // Check if input contains data to prevent program from crashing.
71 2 : if (!newest_cloud.empty())
72 : {
73 : // This function needs a boost shared_pointer to function.
74 : auto cloud_pointer =
75 : boost::make_shared<const pcl::PointCloud<pcl::PointXYZ>>(
76 4 : newest_cloud);
77 : pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_pointer(
78 4 : new pcl::PointCloud<pcl::PointXYZ>);
79 :
80 : // Variables and setup for search algorithm for Euclidian clustering.
81 : pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(
82 4 : new pcl::search::KdTree<pcl::PointXYZ>);
83 2 : kdtree->setInputCloud(cloud_pointer);
84 :
85 : // Euclidean clustering object.
86 4 : pcl::EuclideanClusterExtraction<pcl::PointXYZ> cluster_object;
87 :
88 : // Set cluster tolerance to 2cm (small values may cause objects to be
89 : // divided in several clusters, whereas big values may join objects in a
90 : // same cluster).
91 2 : cluster_object.setClusterTolerance(0.05);
92 :
93 : // Set the minimum and maximum number of points that a cluster can have.
94 2 : cluster_object.setMinClusterSize(2);
95 2 : cluster_object.setMaxClusterSize(100);
96 :
97 : // Set the search method for Euclidian clustering.
98 2 : cluster_object.setSearchMethod(kdtree);
99 :
100 : // Set input for Euclidian clustering.
101 2 : cluster_object.setInputCloud(cloud_pointer);
102 :
103 : // Output vector for detected clusters.
104 4 : std::vector<pcl::PointIndices> clusters;
105 :
106 : // Function to extract clusters from input cloud.
107 2 : cluster_object.extract(clusters);
108 :
109 : // Loop through every found cluster.
110 26 : for (const auto& c : clusters)
111 : {
112 : // Temporary vector for sorting angles to determine if cluster is
113 : // between input angle later on.
114 50 : std::vector<double> min_max_angle;
115 :
116 : // Loop through every point in the cluster.
117 212 : for (const auto& k : c.indices)
118 : {
119 : // Push back x and y of every point in the min_max_angle vector
120 : // so it can be sorted.
121 186 : min_max_angle.push_back(atan2(
122 186 : cloud_pointer->points[static_cast<unsigned long>(k)].y,
123 186 : cloud_pointer->points[static_cast<unsigned long>(k)].x));
124 : }
125 : // Check if min_max_angle contains data.
126 26 : if (!min_max_angle.empty())
127 : {
128 : // Sort angles from low value to high value so the angles can be
129 : // checked later on
130 26 : std::sort(min_max_angle.begin(), min_max_angle.end());
131 :
132 : // Check if angle is bigger than min angle and smaller than max
133 : // angle. If true, the found cluster lies within the search
134 : // angle.
135 52 : if (is_angle_in_angle_range(
136 52 : angle, min_max_angle.front(), min_max_angle.back()))
137 : {
138 : // ++ Pointcloud id counter so each found pointcloud cup
139 : // will be unique.
140 2 : highest_pointcloud_id++;
141 :
142 : // Set current cup id of pointcloud to return.
143 2 : newest_pointcloud.second =
144 : static_cast<unsigned int>(highest_pointcloud_id);
145 :
146 : // Loop through every point in the cluster again.
147 8 : for (const auto& point : c.indices)
148 : {
149 : // Put Points in pointcloud to return because it´s sure
150 : // now this is the right cluster (cup) we are looking
151 : // for.
152 12 : newest_pointcloud.first.points.push_back(
153 : cloud_pointer
154 6 : ->points[static_cast<unsigned long>(point)]);
155 : }
156 :
157 : // Set the rest of the pointcloud parameters.
158 2 : newest_pointcloud.first.width =
159 2 : static_cast<unsigned int>(cloud_pointer->points.size());
160 2 : newest_pointcloud.first.height = 0;
161 2 : newest_pointcloud.first.is_dense = true;
162 :
163 : // Push information back to pointcloud to return.
164 2 : pointcloud_database.push_back(newest_pointcloud);
165 :
166 : // Because the right cluster (cup) is found in the given
167 : // angle break out of loop.
168 2 : break;
169 : }
170 : }
171 : }
172 : }
173 : // Return pointcloud to return.
174 2 : return newest_pointcloud;
175 : }
176 :
177 0 : double LidarInterpreter::translate_kinect_to_lidar(
178 : const std::shared_ptr<cv::Mat>& distance_frame,
179 : const cv::Point& location_cup)
180 : {
181 0 : const int x_coord_cup = location_cup.x;
182 0 : const int x_resolution_img = (*distance_frame).cols;
183 0 : const double viewing_angle_kinect = (70.6 * 3.14159265359) / 180; // degrees
184 0 : const double distance_kinect_lidar = 0.082; // meters
185 : const auto distance_kinect_cup = static_cast<double>(
186 0 : (*distance_frame).at<float>(location_cup.x, location_cup.y));
187 :
188 : const double angle_kinect_cup =
189 0 : std::asin(2 * x_coord_cup
190 0 : * (std::sin(viewing_angle_kinect / 2) / x_resolution_img));
191 :
192 0 : const double angle_lidar_cup =
193 0 : std::atan((distance_kinect_lidar
194 0 : - distance_kinect_cup * std::sin(angle_kinect_cup))
195 0 : / (distance_kinect_cup * std::cos(angle_kinect_cup)));
196 :
197 0 : return angle_lidar_cup;
198 : }
199 :
200 30 : bool LidarInterpreter::is_angle_in_angle_range(double input,
201 : double min_angle,
202 : double max_angle)
203 : {
204 30 : double biggest = 2 * std::acos(-1);
205 30 : if (input < 0)
206 : {
207 26 : input = fmod((2 * std::acos(-1) + fmod(input, 2 * std::acos(-1))),
208 26 : 2 * std::acos(-1));
209 : }
210 30 : if (min_angle < 0)
211 : {
212 16 : min_angle =
213 16 : fmod((2 * std::acos(-1) + fmod(min_angle, 2 * std::acos(-1))),
214 16 : 2 * std::acos(-1));
215 : }
216 30 : if (max_angle < 0)
217 : {
218 12 : max_angle =
219 12 : fmod((2 * std::acos(-1) + fmod(max_angle, 2 * std::acos(-1))),
220 12 : 2 * std::acos(-1));
221 : }
222 :
223 30 : if (input >= min_angle && input >= max_angle)
224 : {
225 0 : biggest = input;
226 : }
227 30 : else if (min_angle >= input && min_angle >= max_angle)
228 : {
229 5 : biggest = min_angle;
230 : }
231 25 : else if (max_angle >= input && max_angle >= min_angle)
232 : {
233 25 : biggest = max_angle;
234 : }
235 :
236 30 : double translation = 2 * std::acos(-1) - biggest;
237 30 : input = fmod((translation + input), 2 * std::acos(-1));
238 30 : min_angle = fmod((translation + min_angle), 2 * std::acos(-1));
239 30 : max_angle = fmod((translation + max_angle), 2 * std::acos(-1));
240 :
241 30 : if (min_angle > max_angle)
242 : {
243 25 : std::swap(min_angle, max_angle);
244 : }
245 30 : if (max_angle > std::acos(-1))
246 : {
247 28 : return input >= max_angle;
248 : }
249 2 : return input <= max_angle;
250 : }
251 :
252 : } // namespace PointCloud
253 3 : } // namespace HLR
|
