ear_clipping_patched.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
37 #include <pcl/surface/ear_clipping.h>
38 #include <pcl/conversions.h>
39 #include <pcl/pcl_config.h>
40 
42 bool
44 {
45  points_.reset (new pcl::PointCloud<pcl::PointXYZ>);
46 
47  if (!MeshProcessing::initCompute ())
48  return (false);
49  fromPCLPointCloud2 (input_mesh_->cloud, *points_);
50 
51  return (true);
52 }
53 
55 void
57 {
58  output.polygons.clear ();
59  output.cloud = input_mesh_->cloud;
60  for (int i = 0; i < static_cast<int> (input_mesh_->polygons.size ()); ++i)
61  triangulate (input_mesh_->polygons[i], output);
62 }
63 
65 void
66 pcl::EarClippingPatched::triangulate (const Vertices& vertices, PolygonMesh& output)
67 {
68  const int n_vertices = static_cast<const int> (vertices.vertices.size ());
69 
70  if (n_vertices < 3)
71  return;
72  else if (n_vertices == 3)
73  {
74  output.polygons.push_back( vertices );
75  return;
76  }
77 
78  std::vector<uint32_t> remaining_vertices (n_vertices);
79  if (area (vertices.vertices) > 0) // clockwise?
80  remaining_vertices = vertices.vertices;
81  else
82  for (int v = 0; v < n_vertices; v++)
83  remaining_vertices[v] = vertices.vertices[n_vertices - 1 - v];
84 
85  // Avoid closed loops.
86  if (remaining_vertices.front () == remaining_vertices.back ())
87  remaining_vertices.erase (remaining_vertices.end () - 1);
88 
89  // null_iterations avoids infinite loops if the polygon is not simple.
90  for (int u = static_cast<int> (remaining_vertices.size ()) - 1, null_iterations = 0;
91  remaining_vertices.size () > 2 && null_iterations < static_cast<int >(remaining_vertices.size () * 2);
92  ++null_iterations, u = (u+1) % static_cast<int> (remaining_vertices.size ()))
93  {
94  int v = (u + 1) % static_cast<int> (remaining_vertices.size ());
95  int w = (u + 2) % static_cast<int> (remaining_vertices.size ());
96 
97  if (isEar (u, v, w, remaining_vertices))
98  {
99  Vertices triangle;
100  triangle.vertices.resize (3);
101  triangle.vertices[0] = remaining_vertices[u];
102  triangle.vertices[1] = remaining_vertices[v];
103  triangle.vertices[2] = remaining_vertices[w];
104  output.polygons.push_back (triangle);
105  remaining_vertices.erase (remaining_vertices.begin () + v);
106  null_iterations = 0;
107  }
108  }
109 }
110 
111 
113 float
114 pcl::EarClippingPatched::area (const std::vector<uint32_t>& vertices)
115 {
116  //if the polygon is projected onto the xy-plane, the area of the polygon is determined
117  //by the trapeze formula of Gauss. However this fails, if the projection is one 'line'.
118  //Therefore the following implementation determines the area of the flat polygon in 3D-space
119  //using Stoke's law: http://code.activestate.com/recipes/578276-3d-polygon-area/
120 
121  int n = static_cast<int> (vertices.size ());
122  float area = 0.0f;
123  Eigen::Vector3f prev_p, cur_p;
124  Eigen::Vector3f total (0,0,0);
125  Eigen::Vector3f unit_normal;
126 
127  if (n > 3)
128  {
129  for (int prev = n - 1, cur = 0; cur < n; prev = cur++)
130  {
131  prev_p = points_->points[vertices[prev]].getVector3fMap();
132  cur_p = points_->points[vertices[cur]].getVector3fMap();
133 
134  total += prev_p.cross( cur_p );
135  }
136 
137  //unit_normal is unit normal vector of plane defined by the first three points
138  prev_p = points_->points[vertices[1]].getVector3fMap() - points_->points[vertices[0]].getVector3fMap();
139  cur_p = points_->points[vertices[2]].getVector3fMap() - points_->points[vertices[0]].getVector3fMap();
140  unit_normal = (prev_p.cross(cur_p)).normalized();
141 
142  area = total.dot( unit_normal );
143  }
144 
145  return area * 0.5f;
146 }
147 
148 
150 bool
151 pcl::EarClippingPatched::isEar (int u, int v, int w, const std::vector<uint32_t>& vertices)
152 {
153  Eigen::Vector3f p_u, p_v, p_w;
154  p_u = points_->points[vertices[u]].getVector3fMap();
155  p_v = points_->points[vertices[v]].getVector3fMap();
156  p_w = points_->points[vertices[w]].getVector3fMap();
157 
158  const float eps = 1e-15f;
159  Eigen::Vector3f p_uv, p_uw;
160  p_uv = p_v - p_u;
161  p_uw = p_w - p_u;
162 
163  // Avoid flat triangles.
164  if ((p_uv.cross(p_uw)).norm() < eps)
165  return (false);
166 
167  Eigen::Vector3f p;
168  // Check if any other vertex is inside the triangle.
169  for (int k = 0; k < static_cast<int> (vertices.size ()); k++)
170  {
171  if ((k == u) || (k == v) || (k == w))
172  continue;
173  p = points_->points[vertices[k]].getVector3fMap();
174 
175  if (isInsideTriangle (p_u, p_v, p_w, p))
176  return (false);
177  }
178  return (true);
179 }
180 
182 bool
184  const Eigen::Vector3f& v,
185  const Eigen::Vector3f& w,
186  const Eigen::Vector3f& p)
187 {
188  // see http://www.blackpawn.com/texts/pointinpoly/default.html
189  // Barycentric Coordinates
190  Eigen::Vector3f v0 = w - u;
191  Eigen::Vector3f v1 = v - u;
192  Eigen::Vector3f v2 = p - u;
193 
194  // Compute dot products
195  float dot00 = v0.dot(v0);
196  float dot01 = v0.dot(v1);
197  float dot02 = v0.dot(v2);
198  float dot11 = v1.dot(v1);
199  float dot12 = v1.dot(v2);
200 
201  // Compute barycentric coordinates
202  float invDenom = 1 / (dot00 * dot11 - dot01 * dot01);
203  float a = (dot11 * dot02 - dot01 * dot12) * invDenom;
204  float b = (dot00 * dot12 - dot01 * dot02) * invDenom;
205 
206  // Check if point is in triangle
207  return (a >= 0) && (b >= 0) && (a + b < 1);
208 }
209 
bool isInsideTriangle(const Eigen::Vector3f &u, const Eigen::Vector3f &v, const Eigen::Vector3f &w, const Eigen::Vector3f &p)
Check if p is inside the triangle (u,v,w).
f
bool initCompute()
This method should get called before starting the actual computation.
float area(const std::vector< uint32_t > &vertices)
Compute the signed area of a polygon.
TFSIMD_FORCE_INLINE Vector3 normalized() const
pcl::PointCloud< pcl::PointXYZ >::Ptr points_
a Pointer to the point cloud data.
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > Vertices
Definition: types.h:48
void triangulate(const Vertices &vertices, PolygonMesh &output)
Triangulate one polygon.
w
bool isEar(int u, int v, int w, const std::vector< uint32_t > &vertices)
Check if the triangle (u,v,w) is an ear.
p
void performProcessing(pcl::PolygonMesh &output)
The actual surface reconstruction method.


jsk_recognition_utils
Author(s):
autogenerated on Fri Dec 6 2019 04:02:51