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 */
52template <class PointType>
53float
54calc_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 */
132template <class PointType>
133std::vector<LineInfo>
134calc_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.