80 bool xy_proj_safe =
true;
81 bool yz_proj_safe =
true;
82 bool xz_proj_safe =
true;
85 PointInT p0 = (*input_)[(*indices_)[0]];
86 PointInT p1 = (*input_)[(*indices_)[indices_->size () - 1]];
87 PointInT p2 = (*input_)[(*indices_)[indices_->size () / 2]];
89 (p1.getVector3fMap() - p0.getVector3fMap()).cross(p2.getVector3fMap() - p0.getVector3fMap()).stableNorm() < Eigen::NumTraits<float>::dummy_precision ())
91 p0 = (*input_)[(*indices_)[rand () % indices_->size ()]];
92 p1 = (*input_)[(*indices_)[rand () % indices_->size ()]];
93 p2 = (*input_)[(*indices_)[rand () % indices_->size ()]];
97 normal_calc_cloud.
resize (3);
98 normal_calc_cloud[0] = p0;
99 normal_calc_cloud[1] = p1;
100 normal_calc_cloud[2] = p2;
102 Eigen::Vector4d normal_calc_centroid;
103 Eigen::Matrix3d normal_calc_covariance;
108 Eigen::Vector3d::Scalar eigen_value;
109 Eigen::Vector3d plane_params;
110 pcl::eigen33 (normal_calc_covariance, eigen_value, plane_params);
111 float theta_x = std::abs (
static_cast<float> (plane_params.dot (x_axis_)));
112 float theta_y = std::abs (
static_cast<float> (plane_params.dot (y_axis_)));
113 float theta_z = std::abs (
static_cast<float> (plane_params.dot (z_axis_)));
117 if (theta_z > projection_angle_thresh_)
119 xz_proj_safe =
false;
120 yz_proj_safe =
false;
122 if (theta_x > projection_angle_thresh_)
124 xz_proj_safe =
false;
125 xy_proj_safe =
false;
127 if (theta_y > projection_angle_thresh_)
129 xy_proj_safe =
false;
130 yz_proj_safe =
false;
134 boolT ismalloc = True;
136 FILE *outfile =
nullptr;
142 const char* flags = qhull_flags.c_str ();
144 FILE *errfile = stderr;
147 coordT *points =
reinterpret_cast<coordT*
> (calloc (indices_->size () * dimension, sizeof (coordT)));
153 for (std::size_t i = 0; i < indices_->size (); ++i, j+=dimension)
155 points[j + 0] =
static_cast<coordT
> ((*input_)[(*indices_)[i]].x);
156 points[j + 1] =
static_cast<coordT
> ((*input_)[(*indices_)[i]].y);
159 else if (yz_proj_safe)
161 for (std::size_t i = 0; i < indices_->size (); ++i, j+=dimension)
163 points[j + 0] =
static_cast<coordT
> ((*input_)[(*indices_)[i]].y);
164 points[j + 1] =
static_cast<coordT
> ((*input_)[(*indices_)[i]].z);
167 else if (xz_proj_safe)
169 for (std::size_t i = 0; i < indices_->size (); ++i, j+=dimension)
171 points[j + 0] =
static_cast<coordT
> ((*input_)[(*indices_)[i]].x);
172 points[j + 1] =
static_cast<coordT
> ((*input_)[(*indices_)[i]].z);
178 PCL_ERROR (
"[pcl::%s::performReconstruction2D] Invalid input!\n", getClassName ().c_str ());
184 qh_zero(qh, errfile);
187 int exitcode = qh_new_qhull (qh, dimension,
static_cast<int> (indices_->size ()), points, ismalloc,
const_cast<char*
> (flags), outfile, errfile);
190 qh_prepare_output(qh);
194 if (exitcode != 0 || qh->num_vertices == 0)
196 PCL_ERROR (
"[pcl::%s::performReconstrution2D] ERROR: qhull was unable to compute a convex hull for the given point cloud (%lu)!\n", getClassName ().c_str (), indices_->size ());
202 qh_freeqhull (qh, !qh_ALL);
203 int curlong, totlong;
204 qh_memfreeshort (qh, &curlong, &totlong);
212 total_area_ = qh->totvol;
216 int num_vertices = qh->num_vertices;
219 hull.
resize(num_vertices, PointInT{});
228 hull[i] = (*input_)[(*indices_)[qh_pointid (qh, vertex->point)]];
229 idx_points[i].first = qh_pointid (qh, vertex->point);
234 Eigen::Vector4f centroid;
238 for (std::size_t j = 0; j < hull.
size (); j++)
240 idx_points[j].second[0] = hull[j].x - centroid[0];
241 idx_points[j].second[1] = hull[j].y - centroid[1];
244 else if (yz_proj_safe)
246 for (std::size_t j = 0; j < hull.
size (); j++)
248 idx_points[j].second[0] = hull[j].y - centroid[1];
249 idx_points[j].second[1] = hull[j].z - centroid[2];
252 else if (xz_proj_safe)
254 for (std::size_t j = 0; j < hull.
size (); j++)
256 idx_points[j].second[0] = hull[j].x - centroid[0];
257 idx_points[j].second[1] = hull[j].z - centroid[2];
263 polygons[0].vertices.resize (hull.
size ());
265 hull_indices_.header = input_->header;
266 hull_indices_.indices.clear ();
267 hull_indices_.indices.reserve (hull.
size ());
269 for (
int j = 0; j < static_cast<int> (hull.
size ()); j++)
271 hull_indices_.indices.push_back ((*indices_)[idx_points[j].first]);
272 hull[j] = (*input_)[(*indices_)[idx_points[j].first]];
273 polygons[0].vertices[j] =
static_cast<unsigned int> (j);
276 qh_freeqhull (qh, !qh_ALL);
277 int curlong, totlong;
278 qh_memfreeshort (qh, &curlong, &totlong);
292 PointCloud &hull, std::vector<pcl::Vertices> &polygons,
bool fill_polygon_data)
297 boolT ismalloc = True;
299 FILE *outfile =
nullptr;
305 const char *flags = qhull_flags.c_str ();
307 FILE *errfile = stderr;
310 coordT *points =
reinterpret_cast<coordT*
> (calloc (indices_->size () * dimension, sizeof (coordT)));
313 for (std::size_t i = 0; i < indices_->size (); ++i, j+=dimension)
315 points[j + 0] =
static_cast<coordT
> ((*input_)[(*indices_)[i]].x);
316 points[j + 1] =
static_cast<coordT
> ((*input_)[(*indices_)[i]].y);
317 points[j + 2] =
static_cast<coordT
> ((*input_)[(*indices_)[i]].z);
323 qh_zero(qh, errfile);
326 int exitcode = qh_new_qhull (qh, dimension,
static_cast<int> (indices_->size ()), points, ismalloc,
const_cast<char*
> (flags), outfile, errfile);
329 qh_prepare_output(qh);
335 PCL_ERROR(
"[pcl::%s::performReconstrution3D] ERROR: qhull was unable to compute a "
336 "convex hull for the given point cloud (%zu)!\n",
337 getClassName().c_str(),
338 static_cast<std::size_t
>(input_->size()));
344 qh_freeqhull (qh, !qh_ALL);
345 int curlong, totlong;
346 qh_memfreeshort (qh, &curlong, &totlong);
353 int num_facets = qh->num_facets;
355 int num_vertices = qh->num_vertices;
356 hull.
resize (num_vertices);
361 unsigned int max_vertex_id = 0;
364 if (vertex->id + 1 > max_vertex_id)
365 max_vertex_id = vertex->id + 1;
369 std::vector<int> qhid_to_pcidx (max_vertex_id);
371 hull_indices_.header = input_->header;
372 hull_indices_.indices.clear ();
373 hull_indices_.indices.reserve (num_vertices);
378 hull_indices_.indices.push_back ((*indices_)[qh_pointid (qh, vertex->point)]);
379 hull[i] = (*input_)[hull_indices_.indices.back ()];
381 qhid_to_pcidx[vertex->id] = i;
387 total_area_ = qh->totarea;
388 total_volume_ = qh->totvol;
391 if (fill_polygon_data)
393 polygons.
resize (num_facets);
399 polygons[dd].vertices.resize (3);
402 int vertex_n, vertex_i;
403 FOREACHvertex_i_ (qh, (*facet).vertices)
405 polygons[dd].vertices[vertex_i] = qhid_to_pcidx[vertex->id];
410 qh_freeqhull (qh, !qh_ALL);
411 int curlong, totlong;
412 qh_memfreeshort (qh, &curlong, &totlong);