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