Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
gp3.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, Willow Garage, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of Willow Garage, Inc. nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 * $Id$
37 *
38 */
39
40#pragma once
41
42// PCL includes
43#include <pcl/surface/reconstruction.h>
44
45#include <pcl/kdtree/kdtree.h>
46
47#include <fstream>
48
49#include <Eigen/Geometry> // for cross
50
51namespace pcl
52{
53 struct PolygonMesh;
54
55 /** \brief Returns if a point X is visible from point R (or the origin)
56 * when taking into account the segment between the points S1 and S2
57 * \param X 2D coordinate of the point
58 * \param S1 2D coordinate of the segment's first point
59 * \param S2 2D coordinate of the segment's second point
60 * \param R 2D coordinate of the reference point (defaults to 0,0)
61 * \ingroup surface
62 */
63 inline bool
64 isVisible (const Eigen::Vector2f &X, const Eigen::Vector2f &S1, const Eigen::Vector2f &S2,
65 const Eigen::Vector2f &R = Eigen::Vector2f::Zero ())
66 {
67 double a0 = S1[1] - S2[1];
68 double b0 = S2[0] - S1[0];
69 double c0 = S1[0]*S2[1] - S2[0]*S1[1];
70 double a1 = -X[1];
71 double b1 = X[0];
72 double c1 = 0;
73 if (R != Eigen::Vector2f::Zero())
74 {
75 a1 += R[1];
76 b1 -= R[0];
77 c1 = R[0]*X[1] - X[0]*R[1];
78 }
79 double div = a0*b1 - b0*a1;
80 double x = (b0*c1 - b1*c0) / div;
81 double y = (a1*c0 - a0*c1) / div;
82
83 bool intersection_outside_XR;
84 if (R == Eigen::Vector2f::Zero())
85 {
86 if (X[0] > 0)
87 intersection_outside_XR = (x <= 0) || (x >= X[0]);
88 else if (X[0] < 0)
89 intersection_outside_XR = (x >= 0) || (x <= X[0]);
90 else if (X[1] > 0)
91 intersection_outside_XR = (y <= 0) || (y >= X[1]);
92 else if (X[1] < 0)
93 intersection_outside_XR = (y >= 0) || (y <= X[1]);
94 else
95 intersection_outside_XR = true;
96 }
97 else
98 {
99 if (X[0] > R[0])
100 intersection_outside_XR = (x <= R[0]) || (x >= X[0]);
101 else if (X[0] < R[0])
102 intersection_outside_XR = (x >= R[0]) || (x <= X[0]);
103 else if (X[1] > R[1])
104 intersection_outside_XR = (y <= R[1]) || (y >= X[1]);
105 else if (X[1] < R[1])
106 intersection_outside_XR = (y >= R[1]) || (y <= X[1]);
107 else
108 intersection_outside_XR = true;
109 }
110 if (intersection_outside_XR)
111 return true;
112 if (S1[0] > S2[0])
113 return (x <= S2[0]) || (x >= S1[0]);
114 if (S1[0] < S2[0])
115 return (x >= S2[0]) || (x <= S1[0]);
116 if (S1[1] > S2[1])
117 return (y <= S2[1]) || (y >= S1[1]);
118 if (S1[1] < S2[1])
119 return (y >= S2[1]) || (y <= S1[1]);
120 return false;
121 }
122
123 /** \brief GreedyProjectionTriangulation is an implementation of a greedy triangulation algorithm for 3D points
124 * based on local 2D projections. It assumes locally smooth surfaces and relatively smooth transitions between
125 * areas with different point densities.
126 * \tparam PointInT Point type must have XYZ and normal information, for example `pcl::PointNormal` or `pcl::PointXYZRGBNormal` or `pcl::PointXYZINormal`
127 * \author Zoltan Csaba Marton
128 * \ingroup surface
129 */
130 template <typename PointInT>
132 {
133 public:
134 using Ptr = shared_ptr<GreedyProjectionTriangulation<PointInT> >;
135 using ConstPtr = shared_ptr<const GreedyProjectionTriangulation<PointInT> >;
136
137 using MeshConstruction<PointInT>::tree_;
138 using MeshConstruction<PointInT>::input_;
139 using MeshConstruction<PointInT>::indices_;
140
142 using KdTreePtr = typename KdTree::Ptr;
143
147
149 {
150 NONE = -1, // not-defined
151 FREE = 0,
152 FRINGE = 1,
154 COMPLETED = 3
155 };
156
157 /** \brief Empty constructor. */
159
160 /** \brief Set the multiplier of the nearest neighbor distance to obtain the final search radius for each point
161 * (this will make the algorithm adapt to different point densities in the cloud).
162 * \param[in] mu the multiplier
163 */
164 inline void
165 setMu (double mu) { mu_ = mu; }
166
167 /** \brief Get the nearest neighbor distance multiplier. */
168 inline double
169 getMu () const { return (mu_); }
170
171 /** \brief Set the maximum number of nearest neighbors to be searched for.
172 * \param[in] nnn the maximum number of nearest neighbors
173 */
174 inline void
175 setMaximumNearestNeighbors (int nnn) { nnn_ = nnn; }
176
177 /** \brief Get the maximum number of nearest neighbors to be searched for. */
178 inline int
179 getMaximumNearestNeighbors () const { return (nnn_); }
180
181 /** \brief Set the sphere radius that is to be used for determining the k-nearest neighbors used for triangulating.
182 * \param[in] radius the sphere radius that is to contain all k-nearest neighbors
183 * \note This distance limits the maximum edge length!
184 */
185 inline void
186 setSearchRadius (double radius) { search_radius_ = radius; }
187
188 /** \brief Get the sphere radius used for determining the k-nearest neighbors. */
189 inline double
190 getSearchRadius () const { return (search_radius_); }
191
192 /** \brief Set the minimum angle each triangle should have.
193 * \param[in] minimum_angle the minimum angle each triangle should have
194 * \note As this is a greedy approach, this will have to be violated from time to time
195 */
196 inline void
197 setMinimumAngle (double minimum_angle) { minimum_angle_ = minimum_angle; }
198
199 /** \brief Get the parameter for distance based weighting of neighbors. */
200 inline double
201 getMinimumAngle () const { return (minimum_angle_); }
202
203 /** \brief Set the maximum angle each triangle can have.
204 * \param[in] maximum_angle the maximum angle each triangle can have
205 * \note For best results, its value should be around 120 degrees
206 */
207 inline void
208 setMaximumAngle (double maximum_angle) { maximum_angle_ = maximum_angle; }
209
210 /** \brief Get the parameter for distance based weighting of neighbors. */
211 inline double
212 getMaximumAngle () const { return (maximum_angle_); }
213
214 /** \brief Don't consider points for triangulation if their normal deviates more than this value from the query point's normal.
215 * \param[in] eps_angle maximum surface angle
216 * \note As normal estimation methods usually give smooth transitions at sharp edges, this ensures correct triangulation
217 * by avoiding connecting points from one side to points from the other through forcing the use of the edge points.
218 */
219 inline void
220 setMaximumSurfaceAngle (double eps_angle) { eps_angle_ = eps_angle; }
221
222 /** \brief Get the maximum surface angle. */
223 inline double
224 getMaximumSurfaceAngle () const { return (eps_angle_); }
225
226 /** \brief Set the flag if the input normals are oriented consistently.
227 * \param[in] consistent set it to true if the normals are consistently oriented
228 */
229 inline void
230 setNormalConsistency (bool consistent) { consistent_ = consistent; }
231
232 /** \brief Get the flag for consistently oriented normals. */
233 inline bool
234 getNormalConsistency () const { return (consistent_); }
235
236 /** \brief Set the flag to order the resulting triangle vertices consistently (positive direction around normal).
237 * @note Assumes consistently oriented normals (towards the viewpoint) -- see setNormalConsistency ()
238 * \param[in] consistent_ordering set it to true if triangle vertices should be ordered consistently
239 */
240 inline void
241 setConsistentVertexOrdering (bool consistent_ordering) { consistent_ordering_ = consistent_ordering; }
242
243 /** \brief Get the flag signaling consistently ordered triangle vertices. */
244 inline bool
246
247 /** \brief Get the state of each point after reconstruction.
248 * \note Options are defined as constants: FREE, FRINGE, COMPLETED, BOUNDARY and NONE
249 */
250 inline std::vector<int>
251 getPointStates () const { return (state_); }
252
253 /** \brief Get the ID of each point after reconstruction.
254 * \note parts are numbered from 0, a -1 denotes unconnected points
255 */
256 inline std::vector<int>
257 getPartIDs () const { return (part_); }
258
259
260 /** \brief Get the sfn list. */
261 inline pcl::Indices
262 getSFN () const { return (sfn_); }
263
264 /** \brief Get the ffn list. */
265 inline pcl::Indices
266 getFFN () const { return (ffn_); }
267
268 protected:
269 /** \brief The nearest neighbor distance multiplier to obtain the final search radius. */
270 double mu_{0.0};
271
272 /** \brief The nearest neighbors search radius for each point and the maximum edge length. */
273 double search_radius_{0.0};
274
275 /** \brief The maximum number of nearest neighbors accepted by searching. */
276 int nnn_{100};
277
278 /** \brief The preferred minimum angle for the triangles. */
280
281 /** \brief The maximum angle for the triangles. */
282 double maximum_angle_{2*M_PI/3};
283
284 /** \brief Maximum surface angle. */
285 double eps_angle_{M_PI/4};
286
287 /** \brief Set this to true if the normals of the input are consistently oriented. */
288 bool consistent_{false};
289
290 /** \brief Set this to true if the output triangle vertices should be consistently oriented. */
292
293 private:
294 /** \brief Struct for storing the angles to nearest neighbors **/
295 struct nnAngle
296 {
297 double angle;
298 pcl::index_t index;
299 pcl::index_t nnIndex;
300 bool visible;
301 };
302
303 /** \brief Struct for storing the edges starting from a fringe point **/
304 struct doubleEdge
305 {
306 doubleEdge () = default;
307 int index{0};
308 Eigen::Vector2f first;
309 Eigen::Vector2f second;
310 };
311
312 // Variables made global to decrease the number of parameters to helper functions
313
314 /** \brief Temporary variable to store a triangle (as a set of point indices) **/
315 pcl::Vertices triangle_{};
316 /** \brief Temporary variable to store point coordinates **/
317 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > coords_{};
318
319 /** \brief A list of angles to neighbors **/
320 std::vector<nnAngle> angles_{};
321 /** \brief Index of the current query point **/
322 pcl::index_t R_{};
323 /** \brief List of point states **/
324 std::vector<int> state_{};
325 /** \brief List of sources **/
326 pcl::Indices source_{};
327 /** \brief List of fringe neighbors in one direction **/
328 pcl::Indices ffn_{};
329 /** \brief List of fringe neighbors in other direction **/
330 pcl::Indices sfn_{};
331 /** \brief Connected component labels for each point **/
332 std::vector<int> part_{};
333 /** \brief Points on the outer edge from which the mesh has to be grown **/
334 std::vector<int> fringe_queue_{};
335
336 /** \brief Flag to set if the current point is free **/
337 bool is_current_free_{false};
338 /** \brief Current point's index **/
339 pcl::index_t current_index_{};
340 /** \brief Flag to set if the previous point is the first fringe neighbor **/
341 bool prev_is_ffn_{false};
342 /** \brief Flag to set if the next point is the second fringe neighbor **/
343 bool prev_is_sfn_{false};
344 /** \brief Flag to set if the next point is the first fringe neighbor **/
345 bool next_is_ffn_{false};
346 /** \brief Flag to set if the next point is the second fringe neighbor **/
347 bool next_is_sfn_{false};
348 /** \brief Flag to set if the first fringe neighbor was changed **/
349 bool changed_1st_fn_{false};
350 /** \brief Flag to set if the second fringe neighbor was changed **/
351 bool changed_2nd_fn_{false};
352 /** \brief New boundary point **/
353 pcl::index_t new2boundary_{};
354
355 /** \brief Flag to set if the next neighbor was already connected in the previous step.
356 * To avoid inconsistency it should not be connected again.
357 */
358 bool already_connected_{false};
359
360 /** \brief Point coordinates projected onto the plane defined by the point normal **/
361 Eigen::Vector3f proj_qp_;
362 /** \brief First coordinate vector of the 2D coordinate frame **/
363 Eigen::Vector3f u_;
364 /** \brief Second coordinate vector of the 2D coordinate frame **/
365 Eigen::Vector3f v_;
366 /** \brief 2D coordinates of the first fringe neighbor **/
367 Eigen::Vector2f uvn_ffn_;
368 /** \brief 2D coordinates of the second fringe neighbor **/
369 Eigen::Vector2f uvn_sfn_;
370 /** \brief 2D coordinates of the first fringe neighbor of the next point **/
371 Eigen::Vector2f uvn_next_ffn_;
372 /** \brief 2D coordinates of the second fringe neighbor of the next point **/
373 Eigen::Vector2f uvn_next_sfn_;
374
375 /** \brief Temporary variable to store 3 coordinates **/
376 Eigen::Vector3f tmp_;
377
378 /** \brief The actual surface reconstruction method.
379 * \param[out] output the resultant polygonal mesh
380 */
381 void
382 performReconstruction (pcl::PolygonMesh &output) override;
383
384 /** \brief The actual surface reconstruction method.
385 * \param[out] polygons the resultant polygons, as a set of vertices. The Vertices structure contains an array of point indices.
386 */
387 void
388 performReconstruction (std::vector<pcl::Vertices> &polygons) override;
389
390 /** \brief The actual surface reconstruction method.
391 * \param[out] polygons the resultant polygons, as a set of vertices. The Vertices structure contains an array of point indices.
392 */
393 bool
394 reconstructPolygons (std::vector<pcl::Vertices> &polygons);
395
396 /** \brief Class get name method. */
397 std::string
398 getClassName () const override { return ("GreedyProjectionTriangulation"); }
399
400 /** \brief Forms a new triangle by connecting the current neighbor to the query point
401 * and the previous neighbor
402 * \param[out] polygons the polygon mesh to be updated
403 * \param[in] prev_index index of the previous point
404 * \param[in] next_index index of the next point
405 * \param[in] next_next_index index of the point after the next one
406 * \param[in] uvn_current 2D coordinate of the current point
407 * \param[in] uvn_prev 2D coordinates of the previous point
408 * \param[in] uvn_next 2D coordinates of the next point
409 */
410 void
411 connectPoint (std::vector<pcl::Vertices> &polygons,
412 const pcl::index_t prev_index,
413 const pcl::index_t next_index,
414 const pcl::index_t next_next_index,
415 const Eigen::Vector2f &uvn_current,
416 const Eigen::Vector2f &uvn_prev,
417 const Eigen::Vector2f &uvn_next);
418
419 /** \brief Whenever a query point is part of a boundary loop containing 3 points, that triangle is created
420 * (called if angle constraints make it possible)
421 * \param[out] polygons the polygon mesh to be updated
422 */
423 void
424 closeTriangle (std::vector<pcl::Vertices> &polygons);
425
426 /** \brief Get the list of containing triangles for each vertex in a PolygonMesh
427 * \param[in] polygonMesh the input polygon mesh
428 */
429 std::vector<std::vector<std::size_t> >
430 getTriangleList (const pcl::PolygonMesh &input);
431
432 /** \brief Add a new triangle to the current polygon mesh
433 * \param[in] a index of the first vertex
434 * \param[in] b index of the second vertex
435 * \param[in] c index of the third vertex
436 * \param[out] polygons the polygon mesh to be updated
437 */
438 inline void
439 addTriangle (pcl::index_t a, pcl::index_t b, pcl::index_t c, std::vector<pcl::Vertices> &polygons)
440 {
441 triangle_.vertices.resize (3);
443 {
444 const PointInT p = input_->at (indices_->at (a));
445 const Eigen::Vector3f pv = p.getVector3fMap ();
446 if (p.getNormalVector3fMap ().dot (
447 (pv - input_->at (indices_->at (b)).getVector3fMap ()).cross (
448 pv - input_->at (indices_->at (c)).getVector3fMap ()) ) > 0)
449 {
450 triangle_.vertices[0] = a;
451 triangle_.vertices[1] = b;
452 triangle_.vertices[2] = c;
453 }
454 else
455 {
456 triangle_.vertices[0] = a;
457 triangle_.vertices[1] = c;
458 triangle_.vertices[2] = b;
459 }
460 }
461 else
462 {
463 triangle_.vertices[0] = a;
464 triangle_.vertices[1] = b;
465 triangle_.vertices[2] = c;
466 }
467 polygons.push_back (triangle_);
468 }
469
470 /** \brief Add a new vertex to the advancing edge front and set its source point
471 * \param[in] v index of the vertex that was connected
472 * \param[in] s index of the source point
473 */
474 inline void
475 addFringePoint (int v, int s)
476 {
477 source_[v] = s;
478 part_[v] = part_[s];
479 fringe_queue_.push_back(v);
480 }
481
482 /** \brief Function for ascending sort of nnAngle, taking visibility into account
483 * (angles to visible neighbors will be first, to the invisible ones after).
484 * \param[in] a1 the first angle
485 * \param[in] a2 the second angle
486 */
487 static inline bool
488 nnAngleSortAsc (const nnAngle& a1, const nnAngle& a2)
489 {
490 if (a1.visible == a2.visible)
491 return (a1.angle < a2.angle);
492 return a1.visible;
493 }
494 };
495
496} // namespace pcl
497
498#ifdef PCL_NO_PRECOMPILE
499#include <pcl/surface/impl/gp3.hpp>
500#endif
GreedyProjectionTriangulation is an implementation of a greedy triangulation algorithm for 3D points ...
Definition gp3.h:132
void setSearchRadius(double radius)
Set the sphere radius that is to be used for determining the k-nearest neighbors used for triangulati...
Definition gp3.h:186
double eps_angle_
Maximum surface angle.
Definition gp3.h:285
double maximum_angle_
The maximum angle for the triangles.
Definition gp3.h:282
double getMaximumAngle() const
Get the parameter for distance based weighting of neighbors.
Definition gp3.h:212
int getMaximumNearestNeighbors() const
Get the maximum number of nearest neighbors to be searched for.
Definition gp3.h:179
void setConsistentVertexOrdering(bool consistent_ordering)
Set the flag to order the resulting triangle vertices consistently (positive direction around normal)...
Definition gp3.h:241
typename PointCloudIn::ConstPtr PointCloudInConstPtr
Definition gp3.h:146
bool getNormalConsistency() const
Get the flag for consistently oriented normals.
Definition gp3.h:234
pcl::Indices getFFN() const
Get the ffn list.
Definition gp3.h:266
bool consistent_ordering_
Set this to true if the output triangle vertices should be consistently oriented.
Definition gp3.h:291
GreedyProjectionTriangulation()=default
Empty constructor.
std::vector< int > getPartIDs() const
Get the ID of each point after reconstruction.
Definition gp3.h:257
shared_ptr< const GreedyProjectionTriangulation< PointInT > > ConstPtr
Definition gp3.h:135
bool getConsistentVertexOrdering() const
Get the flag signaling consistently ordered triangle vertices.
Definition gp3.h:245
int nnn_
The maximum number of nearest neighbors accepted by searching.
Definition gp3.h:276
double getMaximumSurfaceAngle() const
Get the maximum surface angle.
Definition gp3.h:224
pcl::Indices getSFN() const
Get the sfn list.
Definition gp3.h:262
typename PointCloudIn::Ptr PointCloudInPtr
Definition gp3.h:145
double mu_
The nearest neighbor distance multiplier to obtain the final search radius.
Definition gp3.h:270
double search_radius_
The nearest neighbors search radius for each point and the maximum edge length.
Definition gp3.h:273
typename KdTree::Ptr KdTreePtr
Definition gp3.h:142
void setNormalConsistency(bool consistent)
Set the flag if the input normals are oriented consistently.
Definition gp3.h:230
void setMaximumNearestNeighbors(int nnn)
Set the maximum number of nearest neighbors to be searched for.
Definition gp3.h:175
bool consistent_
Set this to true if the normals of the input are consistently oriented.
Definition gp3.h:288
double getSearchRadius() const
Get the sphere radius used for determining the k-nearest neighbors.
Definition gp3.h:190
std::vector< int > getPointStates() const
Get the state of each point after reconstruction.
Definition gp3.h:251
double getMu() const
Get the nearest neighbor distance multiplier.
Definition gp3.h:169
double getMinimumAngle() const
Get the parameter for distance based weighting of neighbors.
Definition gp3.h:201
void setMinimumAngle(double minimum_angle)
Set the minimum angle each triangle should have.
Definition gp3.h:197
double minimum_angle_
The preferred minimum angle for the triangles.
Definition gp3.h:279
shared_ptr< GreedyProjectionTriangulation< PointInT > > Ptr
Definition gp3.h:134
pcl::KdTree< PointInT > KdTree
Definition gp3.h:141
void setMaximumAngle(double maximum_angle)
Set the maximum angle each triangle can have.
Definition gp3.h:208
void setMu(double mu)
Set the multiplier of the nearest neighbor distance to obtain the final search radius for each point ...
Definition gp3.h:165
void setMaximumSurfaceAngle(double eps_angle)
Don't consider points for triangulation if their normal deviates more than this value from the query ...
Definition gp3.h:220
KdTree represents the base spatial locator class for kd-tree implementations.
Definition kdtree.h:56
shared_ptr< KdTree< PointT > > Ptr
Definition kdtree.h:69
MeshConstruction represents a base surface reconstruction class.
PointCloudConstPtr input_
The input point cloud dataset.
Definition pcl_base.h:147
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition pcl_base.h:150
KdTreePtr tree_
A pointer to the spatial search object.
shared_ptr< PointCloud< PointInT > > Ptr
shared_ptr< const PointCloud< PointInT > > ConstPtr
bool isVisible(const Eigen::Vector2f &X, const Eigen::Vector2f &S1, const Eigen::Vector2f &S2, const Eigen::Vector2f &R=Eigen::Vector2f::Zero())
Returns if a point X is visible from point R (or the origin) when taking into account the segment bet...
Definition gp3.h:64
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition types.h:112
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
#define M_PI
Definition pcl_macros.h:203
Describes a set of vertices in a polygon mesh, by basically storing an array of indices.
Definition Vertices.h:15
Indices vertices
Definition Vertices.h:18