Skip to content

Commit b30443e

Browse files
jsprickejspricke
committed
Merge pull request #130 from magro11/master
EarClipping algorithm improved + typing error in Clipper3D
2 parents e539fe4 + f52192a commit b30443e

File tree

3 files changed

+146
-49
lines changed

3 files changed

+146
-49
lines changed

‎surface/include/pcl/surface/ear_clipping.h

Lines changed: 4 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -106,11 +106,10 @@ namespace pcl
106106
* \param[in] p the point to check
107107
*/
108108
bool
109-
isInsideTriangle (const Eigen::Vector2f& u,
110-
const Eigen::Vector2f& v,
111-
const Eigen::Vector2f& w,
112-
const Eigen::Vector2f& p);
113-
109+
isInsideTriangle (const Eigen::Vector3f& u,
110+
const Eigen::Vector3f& v,
111+
const Eigen::Vector3f& w,
112+
const Eigen::Vector3f& p);
114113

115114
/** \brief Compute the cross product between 2D vectors.
116115
* \param[in] p1 the first 2D vector

‎surface/src/ear_clipping.cpp

Lines changed: 69 additions & 44 deletions
Original file line numberDiff line numberDiff line change
@@ -68,8 +68,13 @@ pcl::EarClipping::triangulate (const Vertices& vertices, PolygonMesh& output)
6868
{
6969
const int n_vertices = static_cast<const int> (vertices.vertices.size ());
7070

71-
if (n_vertices <= 3)
71+
if (n_vertices < 3)
7272
return;
73+
else if (n_vertices == 3)
74+
{
75+
output.polygons.push_back( vertices );
76+
return;
77+
}
7378

7479
std::vector<uint32_t> remaining_vertices (n_vertices);
7580
if (area (vertices.vertices) > 0) // clockwise?
@@ -109,51 +114,64 @@ pcl::EarClipping::triangulate (const Vertices& vertices, PolygonMesh& output)
109114
float
110115
pcl::EarClipping::area (const std::vector<uint32_t>& vertices)
111116
{
112-
int n = static_cast<int> (vertices.size ());
113-
float area = 0.0f;
114-
Eigen::Vector2f prev_p, cur_p;
115-
for (int prev = n - 1, cur = 0; cur < n; prev = cur++)
116-
{
117-
prev_p[0] = points_->points[vertices[prev]].x;
118-
prev_p[1] = points_->points[vertices[prev]].y;
119-
cur_p[0] = points_->points[vertices[cur]].x;
120-
cur_p[1] = points_->points[vertices[cur]].y;
117+
//if the polygon is projected onto the xy-plane, the area of the polygon is determined
118+
//by the trapeze formula of Gauss. However this fails, if the projection is one 'line'.
119+
//Therefore the following implementation determines the area of the flat polygon in 3D-space
120+
//using Stoke's law: http://code.activestate.com/recipes/578276-3d-polygon-area/
121+
122+
int n = static_cast<int> (vertices.size ());
123+
float area = 0.0f;
124+
Eigen::Vector3f prev_p, cur_p;
125+
Eigen::Vector3f total (0,0,0);
126+
Eigen::Vector3f unit_normal;
127+
128+
if (n > 3)
129+
{
130+
for (int prev = n - 1, cur = 0; cur < n; prev = cur++)
131+
{
132+
prev_p = points_->points[vertices[prev]].getVector3fMap();
133+
cur_p = points_->points[vertices[cur]].getVector3fMap();
121134

122-
area += crossProduct (prev_p, cur_p);
123-
}
124-
return (area * 0.5f);
135+
total += prev_p.cross( cur_p );
136+
}
137+
138+
//unit_normal is unit normal vector of plane defined by the first three points
139+
prev_p = points_->points[vertices[1]].getVector3fMap() - points_->points[vertices[0]].getVector3fMap();
140+
cur_p = points_->points[vertices[2]].getVector3fMap() - points_->points[vertices[0]].getVector3fMap();
141+
unit_normal = (prev_p.cross(cur_p)).normalized();
142+
143+
area = total.dot( unit_normal );
144+
}
145+
146+
return area * 0.5f;
125147
}
126148

127149

128150
/////////////////////////////////////////////////////////////////////////////////////////////
129151
bool
130152
pcl::EarClipping::isEar (int u, int v, int w, const std::vector<uint32_t>& vertices)
131153
{
132-
Eigen::Vector2f p_u, p_v, p_w;
133-
p_u[0] = points_->points[vertices[u]].x;
134-
p_u[1] = points_->points[vertices[u]].y;
135-
p_v[0] = points_->points[vertices[v]].x;
136-
p_v[1] = points_->points[vertices[v]].y;
137-
p_w[0] = points_->points[vertices[w]].x;
138-
p_w[1] = points_->points[vertices[w]].y;
154+
Eigen::Vector3f p_u, p_v, p_w;
155+
p_u = points_->points[vertices[u]].getVector3fMap();
156+
p_v = points_->points[vertices[v]].getVector3fMap();
157+
p_w = points_->points[vertices[w]].getVector3fMap();
139158

140-
// Avoid flat triangles.
141-
// FIXME: triangulation would fail if all the triangles are flat in the X-Y axis
142159
const float eps = 1e-15f;
143-
Eigen::Vector2f p_uv, p_uw;
160+
Eigen::Vector3f p_uv, p_uw;
144161
p_uv = p_v - p_u;
145162
p_uw = p_w - p_u;
146-
if (crossProduct (p_uv, p_uw) < eps)
163+
164+
// Avoid flat triangles.
165+
if ((p_uv.cross(p_uw)).norm() < eps)
147166
return (false);
148167

149-
Eigen::Vector2f p;
168+
Eigen::Vector3f p;
150169
// Check if any other vertex is inside the triangle.
151170
for (int k = 0; k < static_cast<int> (vertices.size ()); k++)
152171
{
153172
if ((k == u) || (k == v) || (k == w))
154173
continue;
155-
p[0] = points_->points[vertices[k]].x;
156-
p[1] = points_->points[vertices[k]].y;
174+
p = points_->points[vertices[k]].getVector3fMap();
157175

158176
if (isInsideTriangle (p_u, p_v, p_w, p))
159177
return (false);
@@ -163,23 +181,30 @@ pcl::EarClipping::isEar (int u, int v, int w, const std::vector<uint32_t>& verti
163181

164182
/////////////////////////////////////////////////////////////////////////////////////////////
165183
bool
166-
pcl::EarClipping::isInsideTriangle (const Eigen::Vector2f& u,
167-
const Eigen::Vector2f& v,
168-
const Eigen::Vector2f& w,
169-
const Eigen::Vector2f& p)
184+
pcl::EarClipping::isInsideTriangle (const Eigen::Vector3f& u,
185+
const Eigen::Vector3f& v,
186+
const Eigen::Vector3f& w,
187+
const Eigen::Vector3f& p)
170188
{
171-
// Check first side.
172-
if (crossProduct (w - v, p - v) < 0)
173-
return (false);
174-
175-
// Check second side.
176-
if (crossProduct (v - u, p - u) < 0)
177-
return (false);
178-
179-
// Check third side.
180-
if (crossProduct (u - w, p - w) < 0)
181-
return (false);
182-
183-
return (true);
189+
// see http://www.blackpawn.com/texts/pointinpoly/default.html
190+
// Barycentric Coordinates
191+
Eigen::Vector3f v0 = w - u;
192+
Eigen::Vector3f v1 = v - u;
193+
Eigen::Vector3f v2 = p - u;
194+
195+
// Compute dot products
196+
float dot00 = v0.dot(v0);
197+
float dot01 = v0.dot(v1);
198+
float dot02 = v0.dot(v2);
199+
float dot11 = v1.dot(v1);
200+
float dot12 = v1.dot(v2);
201+
202+
// Compute barycentric coordinates
203+
float invDenom = 1 / (dot00 * dot11 - dot01 * dot01);
204+
float a = (dot11 * dot02 - dot01 * dot12) * invDenom;
205+
float b = (dot00 * dot12 - dot01 * dot02) * invDenom;
206+
207+
// Check if point is in triangle
208+
return (a >= 0) && (b >= 0) && (a + b < 1);
184209
}
185210

‎test/surface/test_ear_clipping.cpp

Lines changed: 73 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -106,6 +106,79 @@ TEST (PCL, EarClipping)
106106
}
107107
}
108108

109+
TEST (PCL, EarClippingCubeTest)
110+
{
111+
PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>());
112+
cloud->height = 1;
113+
//bottom of cube (z=0)
114+
cloud->points.push_back (PointXYZ ( 0.f, 0.f, 0.f));
115+
cloud->points.push_back (PointXYZ ( 1.f, 0.f, 0.f));
116+
cloud->points.push_back (PointXYZ ( 1.f, 1.f, 0.f));
117+
cloud->points.push_back (PointXYZ ( 0.f, 1.f, 0.f));
118+
//top of cube (z=1.0)
119+
cloud->points.push_back (PointXYZ ( 0.f, 0.f, 1.f));
120+
cloud->points.push_back (PointXYZ ( 1.f, 0.f, 1.f));
121+
cloud->points.push_back (PointXYZ ( 1.f, 1.f, 1.f));
122+
cloud->points.push_back (PointXYZ ( 0.f, 1.f, 1.f));
123+
cloud->width = static_cast<uint32_t> (cloud->points.size ());
124+
125+
Vertices vertices;
126+
vertices.vertices.resize(4);
127+
128+
const int squares[][4] = { {1, 5, 6, 2},
129+
{2, 6, 7, 3},
130+
{3, 7, 4, 0},
131+
{0, 4, 5, 1},
132+
{4, 7, 6, 5},
133+
{0, 1, 2, 3} };
134+
135+
const int truth[][3] = { {2, 1, 5},
136+
{6, 2, 5},
137+
{3, 2, 6},
138+
{7, 3, 6},
139+
{0, 3, 7},
140+
{4, 0, 7},
141+
{1, 0, 4},
142+
{5, 1, 4},
143+
{5, 4, 7},
144+
{6, 5, 7},
145+
{3, 0, 1},
146+
{2, 3, 1} };
147+
148+
PolygonMesh::Ptr mesh (new PolygonMesh);
149+
toPCLPointCloud2 (*cloud, mesh->cloud);
150+
151+
for (int i = 0; i < 6; ++i)
152+
{
153+
vertices.vertices[0] = squares[i][0];
154+
vertices.vertices[1] = squares[i][1];
155+
vertices.vertices[2] = squares[i][2];
156+
vertices.vertices[3] = squares[i][3];
157+
mesh->polygons.push_back (vertices);
158+
}
159+
160+
EarClipping clipper;
161+
PolygonMesh::ConstPtr mesh_aux (mesh);
162+
clipper.setInputMesh (mesh_aux);
163+
164+
PolygonMesh triangulated_mesh;
165+
clipper.process (triangulated_mesh);
166+
167+
EXPECT_EQ (triangulated_mesh.polygons.size (), 12);
168+
for (int i = 0; i < static_cast<int> (triangulated_mesh.polygons.size ()); ++i)
169+
EXPECT_EQ (triangulated_mesh.polygons[i].vertices.size (), 3);
170+
171+
172+
173+
for (int pi = 0; pi < static_cast<int> (triangulated_mesh.polygons.size ()); ++pi)
174+
{
175+
for (int vi = 0; vi < 3; ++vi)
176+
{
177+
EXPECT_EQ (triangulated_mesh.polygons[pi].vertices[vi], truth[pi][vi]);
178+
}
179+
}
180+
}
181+
109182
/* ---[ */
110183
int
111184
main (int argc, char** argv)

0 commit comments

Comments
 (0)