22 #ifndef __PLUGINS_LASER_LINES_LINE_FUNC_H_ 23 #define __PLUGINS_LASER_LINES_LINE_FUNC_H_ 25 #include "line_info.h" 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> 52 template <
class Po
intType>
55 pcl::ModelCoefficients::Ptr coeff,
56 Eigen::Vector3f &end_point_1, Eigen::Vector3f &end_point_2)
58 if (cloud_line->points.size() < 2)
return 0.;
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);
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];
77 ssize_t idx_1 = 0, idx_2 = 0;
78 float max_dist_1 = 0.f, max_dist_2 = 0.f;
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);
87 if (dist > max_dist_1) {
93 if (dist > max_dist_2) {
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];
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);
110 return (ptv_1 - ptv_2).norm();
131 template <
class Po
intType>
132 std::vector<LineInfo>
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,
144 pcl::PassThrough<PointType> passthrough;
145 passthrough.setInputCloud(input);
146 passthrough.filter(*in_cloud);
149 pcl::ModelCoefficients::Ptr coeff(
new pcl::ModelCoefficients());
150 pcl::PointIndices::Ptr inliers(
new pcl::PointIndices());
152 std::vector<LineInfo> linfos;
154 while (in_cloud->points.size () > segm_min_inliers) {
159 typename pcl::search::KdTree<PointType>::Ptr
160 search(
new pcl::search::KdTree<PointType>);
161 search->setInputCloud(in_cloud);
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) {
178 if ((
double)inliers->indices.size() < segm_min_inliers) {
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);
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);
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]));
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;
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);
238 extract.setNegative(
true);
239 extract.filter(*cloud_f);
240 *in_cloud = *cloud_f;
242 if (!line_cluster_index || line_cluster_index->indices.empty())
continue;
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);
249 (min_length >= 0 && length < min_length) ||
250 (max_length >= 0 && length > max_length))
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());
278 if ((min_dist >= 0. && dist < min_dist) ||
279 (max_dist >= 0. && dist > max_dist))
290 float line_dir_k = ld_unit.dot(end_point_2 - end_point_1);
292 if (line_dir_k >= 0) {
300 coeff->values[0] = P[0];
301 coeff->values[1] = P[1];
302 coeff->values[2] = P[2];
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);
311 linfos.push_back(info);
314 if (remaining_cloud) {
315 *remaining_cloud = *in_cloud;
Line information container.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW float bearing
bearing to point on line
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
Eigen::Vector3f end_point_2
line segment end point
Eigen::Vector3f base_point
optimized closest point on line
Eigen::Vector3f line_direction
line direction vector
float length
length of the detecte line segment
Eigen::Vector3f point_on_line
point on line vector
pcl::PointCloud< pcl::PointXYZ >::Ptr cloud
point cloud consisting only of points account to this line