Fawkes API  Fawkes Development Version
line_func.h
1 
2 /***************************************************************************
3  * line_func.h - function to detect lines in laser data
4  *
5  * Created: Tue Mar 17 11:13:24 2015 (re-factoring)
6  * Copyright 2011-2015 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #ifndef _PLUGINS_LASER_LINES_LINE_FUNC_H_
23 #define _PLUGINS_LASER_LINES_LINE_FUNC_H_
24 
25 #include "line_info.h"
26 
27 #include <pcl/common/centroid.h>
28 #include <pcl/common/distances.h>
29 #include <pcl/common/transforms.h>
30 #include <pcl/filters/conditional_removal.h>
31 #include <pcl/filters/extract_indices.h>
32 #include <pcl/filters/passthrough.h>
33 #include <pcl/filters/project_inliers.h>
34 #include <pcl/point_cloud.h>
35 #include <pcl/point_types.h>
36 #include <pcl/sample_consensus/method_types.h>
37 #include <pcl/sample_consensus/model_types.h>
38 #include <pcl/search/kdtree.h>
39 #include <pcl/segmentation/extract_clusters.h>
40 #include <pcl/segmentation/sac_segmentation.h>
41 #include <pcl/surface/convex_hull.h>
42 
43 /** Calculate length of line from associated points.
44  * The unit depends on the units of the input data.
45  * @param cloud_line point cloud with points from which the line model was
46  * determined.
47  * @param coeff line model coefficients
48  * @param end_point_1 upon return contains one of the end points of the line segment
49  * @param end_point_2 upon return contains the second end point of the line segment
50  * @return length of line
51  */
52 template <class PointType>
53 float
54 calc_line_length(typename pcl::PointCloud<PointType>::Ptr cloud_line,
55  pcl::ModelCoefficients::Ptr coeff,
56  Eigen::Vector3f & end_point_1,
57  Eigen::Vector3f & end_point_2)
58 {
59  if (cloud_line->points.size() < 2)
60  return 0.;
61 
62  // Project the model inliers
63  typename pcl::PointCloud<PointType>::Ptr cloud_line_proj(new pcl::PointCloud<PointType>());
64  pcl::ProjectInliers<PointType> proj;
65  proj.setModelType(pcl::SACMODEL_LINE);
66  proj.setInputCloud(cloud_line);
67  proj.setModelCoefficients(coeff);
68  proj.filter(*cloud_line_proj);
69 
70  Eigen::Vector3f point_on_line, line_dir;
71  point_on_line[0] = cloud_line_proj->points[0].x;
72  point_on_line[1] = cloud_line_proj->points[0].y;
73  point_on_line[2] = cloud_line_proj->points[0].z;
74  line_dir[0] = coeff->values[3];
75  line_dir[1] = coeff->values[4];
76  line_dir[2] = coeff->values[5];
77  line_dir.normalize();
78 
79  ssize_t idx_1 = 0, idx_2 = 0;
80  float max_dist_1 = 0.f, max_dist_2 = 0.f;
81 
82  for (size_t i = 1; i < cloud_line_proj->points.size(); ++i) {
83  const PointType &pt = cloud_line_proj->points[i];
84  Eigen::Vector3f ptv(pt.x, pt.y, pt.z);
85  Eigen::Vector3f diff(ptv - point_on_line);
86  float dist = diff.norm();
87  float dir = line_dir.dot(diff);
88  if (dir >= 0) {
89  if (dist > max_dist_1) {
90  max_dist_1 = dist;
91  idx_1 = i;
92  }
93  }
94  if (dir <= 0) {
95  if (dist > max_dist_2) {
96  max_dist_2 = dist;
97  idx_2 = i;
98  }
99  }
100  }
101 
102  if (idx_1 >= 0 && idx_2 >= 0) {
103  const PointType &pt_1 = cloud_line_proj->points[idx_1];
104  const PointType &pt_2 = cloud_line_proj->points[idx_2];
105 
106  Eigen::Vector3f ptv_1(pt_1.x, pt_1.y, pt_1.z);
107  Eigen::Vector3f ptv_2(pt_2.x, pt_2.y, pt_2.z);
108 
109  end_point_1 = ptv_1;
110  end_point_2 = ptv_2;
111 
112  return (ptv_1 - ptv_2).norm();
113  } else {
114  return 0.f;
115  }
116 }
117 
118 /** Calculate a number of lines from a given point cloud.
119  * @param input input point clouds from which to extract lines
120  * @param segm_min_inliers minimum total number of required inliers to consider a line
121  * @param segm_max_iterations maximum number of line RANSAC iterations
122  * @param segm_distance_threshold maximum distance of point to line to account it to a line
123  * @param segm_sample_max_dist max inter-sample distance for line RANSAC
124  * @param min_length minimum length of line to consider it
125  * @param max_length maximum length of a line to consider it
126  * @param min_dist minimum distance from frame origin to closest point on line to consider it
127  * @param max_dist maximum distance from frame origin to closest point on line to consider it
128  * @param remaining_cloud if passed with a valid cloud will be assigned the remaining
129  * points, that is points which have not been accounted to a line, upon return
130  * @return vector of info about detected lines
131  */
132 template <class PointType>
133 std::vector<LineInfo>
134 calc_lines(typename pcl::PointCloud<PointType>::ConstPtr input,
135  unsigned int segm_min_inliers,
136  unsigned int segm_max_iterations,
137  float segm_distance_threshold,
138  float segm_sample_max_dist,
139  float cluster_tolerance,
140  float cluster_quota,
141  float min_length,
142  float max_length,
143  float min_dist,
144  float max_dist,
145  typename pcl::PointCloud<PointType>::Ptr remaining_cloud =
147 {
149 
150  {
151  // Erase non-finite points
152  pcl::PassThrough<PointType> passthrough;
153  passthrough.setInputCloud(input);
154  passthrough.filter(*in_cloud);
155  }
156 
157  pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients());
158  pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
159 
160  std::vector<LineInfo> linfos;
161 
162  while (in_cloud->points.size() > segm_min_inliers) {
163  // Segment the largest linear component from the remaining cloud
164  //logger->log_info(name(), "[L %u] %zu points left",
165  // loop_count_, in_cloud->points.size());
166 
167  typename pcl::search::KdTree<PointType>::Ptr search(new pcl::search::KdTree<PointType>);
168  search->setInputCloud(in_cloud);
169 
170  pcl::SACSegmentation<PointType> seg;
171  seg.setOptimizeCoefficients(true);
172  seg.setModelType(pcl::SACMODEL_LINE);
173  seg.setMethodType(pcl::SAC_RANSAC);
174  seg.setMaxIterations(segm_max_iterations);
175  seg.setDistanceThreshold(segm_distance_threshold);
176  seg.setSamplesMaxDist(segm_sample_max_dist, search);
177  seg.setInputCloud(in_cloud);
178  seg.segment(*inliers, *coeff);
179  if (inliers->indices.size() == 0) {
180  // no line found
181  break;
182  }
183 
184  // check for a minimum number of expected inliers
185  if ((double)inliers->indices.size() < segm_min_inliers) {
186  //logger->log_warn(name(), "[L %u] no more lines (%zu inliers, required %u)",
187  // loop_count_, inliers->indices.size(), segm_min_inliers);
188  break;
189  }
190 
191  //logger->log_info(name(), "[L %u] Found line with %zu inliers",
192  // loop_count_, inliers->indices.size());
193 
194  // Cluster within the line to make sure it is a contiguous line
195  // the line search can output a line which combines lines at separate
196  // ends of the field of view...
197 
198  typename pcl::search::KdTree<PointType>::Ptr kdtree_line_cluster(
199  new pcl::search::KdTree<PointType>());
200  typename pcl::search::KdTree<PointType>::IndicesConstPtr search_indices(
201  new std::vector<int>(inliers->indices));
202  kdtree_line_cluster->setInputCloud(in_cloud, search_indices);
203 
204  std::vector<pcl::PointIndices> line_cluster_indices;
205  pcl::EuclideanClusterExtraction<PointType> line_ec;
206  line_ec.setClusterTolerance(cluster_tolerance);
207  size_t min_size = (size_t)floorf(cluster_quota * inliers->indices.size());
208  line_ec.setMinClusterSize(min_size);
209  line_ec.setMaxClusterSize(inliers->indices.size());
210  line_ec.setSearchMethod(kdtree_line_cluster);
211  line_ec.setInputCloud(in_cloud);
212  line_ec.setIndices(inliers);
213  line_ec.extract(line_cluster_indices);
214 
215  pcl::PointIndices::Ptr line_cluster_index;
216  if (!line_cluster_indices.empty()) {
217  line_cluster_index = pcl::PointIndices::Ptr(new pcl::PointIndices(line_cluster_indices[0]));
218  }
219 
220  // re-calculate coefficients based on line cluster only
221  if (line_cluster_index) {
222  pcl::SACSegmentation<PointType> segc;
223  segc.setOptimizeCoefficients(true);
224  segc.setModelType(pcl::SACMODEL_LINE);
225  segc.setMethodType(pcl::SAC_RANSAC);
226  segc.setMaxIterations(segm_max_iterations);
227  segc.setDistanceThreshold(segm_distance_threshold);
228  segc.setInputCloud(in_cloud);
229  segc.setIndices(line_cluster_index);
230  pcl::PointIndices::Ptr tmp_index(new pcl::PointIndices());
231  segc.segment(*tmp_index, *coeff);
232  *line_cluster_index = *tmp_index;
233  }
234 
235  // Remove the linear or clustered inliers, extract the rest
238  pcl::ExtractIndices<PointType> extract;
239  extract.setInputCloud(in_cloud);
240  extract.setIndices(
241  (line_cluster_index && !line_cluster_index->indices.empty()) ? line_cluster_index : inliers);
242  extract.setNegative(false);
243  extract.filter(*cloud_line);
244 
245  extract.setNegative(true);
246  extract.filter(*cloud_f);
247  *in_cloud = *cloud_f;
248 
249  if (!line_cluster_index || line_cluster_index->indices.empty())
250  continue;
251 
252  // Check if this line has the requested minimum length
253  Eigen::Vector3f end_point_1, end_point_2;
254  float length = calc_line_length<PointType>(cloud_line, coeff, end_point_1, end_point_2);
255 
256  if (length == 0 || (min_length >= 0 && length < min_length)
257  || (max_length >= 0 && length > max_length)) {
258  continue;
259  }
260 
261  LineInfo info;
262  info.cloud.reset(new pcl::PointCloud<PointType>());
263 
264  info.point_on_line[0] = coeff->values[0];
265  info.point_on_line[1] = coeff->values[1];
266  info.point_on_line[2] = coeff->values[2];
267  info.line_direction[0] = coeff->values[3];
268  info.line_direction[1] = coeff->values[4];
269  info.line_direction[2] = coeff->values[5];
270 
271  info.length = length;
272 
273  Eigen::Vector3f ld_unit = info.line_direction / info.line_direction.norm();
274  Eigen::Vector3f pol_invert = Eigen::Vector3f(0, 0, 0) - info.point_on_line;
275  Eigen::Vector3f P = info.point_on_line + pol_invert.dot(ld_unit) * ld_unit;
276  Eigen::Vector3f x_axis(1, 0, 0);
277  info.bearing = acosf(x_axis.dot(P) / P.norm());
278  // we also want to encode the direction of the angle
279  if (P[1] < 0)
280  info.bearing = fabs(info.bearing) * -1.;
281 
282  info.base_point = P;
283  float dist = info.base_point.norm();
284 
285  if ((min_dist >= 0. && dist < min_dist) || (max_dist >= 0. && dist > max_dist)) {
286  //logger->log_warn(name(), "[L %u] line too close or too far (%f, min %f, max %f)",
287  // loop_count_, dist, min_dist, max_dist);
288  continue;
289  }
290 
291  // Calculate parameter for e2 with respect to e1 as origin and the
292  // direction vector, i.e. a k such that e1 + k * dir == e2.
293  // If the resulting parameter is >= 0, then the direction vector
294  // points from e1 to e2, otherwise it points from e2 to e1.
295  float line_dir_k = ld_unit.dot(end_point_2 - end_point_1);
296 
297  if (line_dir_k >= 0) {
298  info.end_point_1 = end_point_1;
299  info.end_point_2 = end_point_2;
300  } else {
301  info.end_point_1 = end_point_2;
302  info.end_point_2 = end_point_1;
303  }
304 
305  coeff->values[0] = P[0];
306  coeff->values[1] = P[1];
307  coeff->values[2] = P[2];
308 
309  // Project the model inliers
310  pcl::ProjectInliers<PointType> proj;
311  proj.setModelType(pcl::SACMODEL_LINE);
312  proj.setInputCloud(cloud_line);
313  proj.setModelCoefficients(coeff);
314  proj.filter(*info.cloud);
315 
316  linfos.push_back(info);
317  }
318 
319  if (remaining_cloud) {
320  *remaining_cloud = *in_cloud;
321  }
322 
323  return linfos;
324 }
325 
326 #endif
Line information container.
Definition: line_info.h:40
Eigen::Vector3f point_on_line
point on line vector
Definition: line_info.h:47
float length
length of the detecte line segment
Definition: line_info.h:45
pcl::PointCloud< pcl::PointXYZ >::Ptr cloud
point cloud consisting only of points account to this line
Definition: line_info.h:55
Eigen::Vector3f line_direction
line direction vector
Definition: line_info.h:48
Eigen::Vector3f end_point_1
line segment end point
Definition: line_info.h:52
EIGEN_MAKE_ALIGNED_OPERATOR_NEW float bearing
bearing to point on line
Definition: line_info.h:44
Eigen::Vector3f base_point
optimized closest point on line
Definition: line_info.h:50
Eigen::Vector3f end_point_2
line segment end point
Definition: line_info.h:53
This class tries to translate the found plan to interpreteable data for the rest of the program.