22#ifndef _PLUGINS_LASER_LINES_LINE_FUNC_H_
23#define _PLUGINS_LASER_LINES_LINE_FUNC_H_
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>
52template <
class Po
intType>
55 pcl::ModelCoefficients::Ptr coeff,
56 Eigen::Vector3f & end_point_1,
57 Eigen::Vector3f & end_point_2)
59 if (cloud_line->points.size() < 2)
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);
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];
79 ssize_t idx_1 = 0, idx_2 = 0;
80 float max_dist_1 = 0.f, max_dist_2 = 0.f;
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);
89 if (dist > max_dist_1) {
95 if (dist > max_dist_2) {
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];
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);
112 return (ptv_1 - ptv_2).norm();
132template <
class Po
intType>
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,
152 pcl::PassThrough<PointType> passthrough;
153 passthrough.setInputCloud(input);
154 passthrough.filter(*in_cloud);
157 pcl::ModelCoefficients::Ptr coeff(
new pcl::ModelCoefficients());
158 pcl::PointIndices::Ptr inliers(
new pcl::PointIndices());
160 std::vector<LineInfo> linfos;
162 while (in_cloud->points.size() > segm_min_inliers) {
167 typename pcl::search::KdTree<PointType>::Ptr
search(
new pcl::search::KdTree<PointType>);
168 search->setInputCloud(in_cloud);
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) {
185 if ((
double)inliers->indices.size() < segm_min_inliers) {
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);
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);
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]));
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;
238 pcl::ExtractIndices<PointType> extract;
239 extract.setInputCloud(in_cloud);
241 (line_cluster_index && !line_cluster_index->indices.empty()) ? line_cluster_index : inliers);
242 extract.setNegative(
false);
243 extract.filter(*cloud_line);
245 extract.setNegative(
true);
246 extract.filter(*cloud_f);
247 *in_cloud = *cloud_f;
249 if (!line_cluster_index || line_cluster_index->indices.empty())
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);
256 if (length == 0 || (min_length >= 0 && length < min_length)
257 || (max_length >= 0 && length > max_length)) {
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());
285 if ((min_dist >= 0. && dist < min_dist) || (max_dist >= 0. && dist > max_dist)) {
295 float line_dir_k = ld_unit.dot(end_point_2 - end_point_1);
297 if (line_dir_k >= 0) {
305 coeff->values[0] = P[0];
306 coeff->values[1] = P[1];
307 coeff->values[2] = P[2];
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);
316 linfos.push_back(info);
319 if (remaining_cloud) {
320 *remaining_cloud = *in_cloud;
Line information container.
Eigen::Vector3f point_on_line
point on line vector
float length
length of the detecte line segment
pcl::PointCloud< pcl::PointXYZ >::Ptr cloud
point cloud consisting only of points account to this line
Eigen::Vector3f line_direction
line direction vector
Eigen::Vector3f end_point_1
line segment end point
EIGEN_MAKE_ALIGNED_OPERATOR_NEW float bearing
bearing to point on line
Eigen::Vector3f base_point
optimized closest point on line
Eigen::Vector3f end_point_2
line segment end point
This class tries to translate the found plan to interpreteable data for the rest of the program.