Skip to content

Commit

Permalink
Merge pull request #130 from magro11/master
Browse files Browse the repository at this point in the history 
EarClipping algorithm improved + typing error in Clipper3D
  • Loading branch information
@jspricke
jspricke committed Oct 11, 2013
2 parents e539fe4 + f52192a commit b30443e
Show file tree
Hide file tree
Showing 3 changed files with 146 additions and 49 deletions.
9 changes: 4 additions & 5 deletions surface/include/pcl/surface/ear_clipping.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -106,11 +106,10 @@ namespace pcl
* \param[in] p the point to check
*/
bool
isInsideTriangle (const Eigen::Vector2f& u,
const Eigen::Vector2f& v,
const Eigen::Vector2f& w,
const Eigen::Vector2f& p);

isInsideTriangle (const Eigen::Vector3f& u,
const Eigen::Vector3f& v,
const Eigen::Vector3f& w,
const Eigen::Vector3f& p);

/** \brief Compute the cross product between 2D vectors.
* \param[in] p1 the first 2D vector
Expand Down
113 changes: 69 additions & 44 deletions surface/src/ear_clipping.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,8 +68,13 @@ pcl::EarClipping::triangulate (const Vertices& vertices, PolygonMesh& output)
{
const int n_vertices = static_cast<const int> (vertices.vertices.size ());

if (n_vertices <= 3)
if (n_vertices < 3)
return;
else if (n_vertices == 3)
{
output.polygons.push_back( vertices );
return;
}

std::vector<uint32_t> remaining_vertices (n_vertices);
if (area (vertices.vertices) > 0) // clockwise?
Expand Down Expand Up @@ -109,51 +114,64 @@ pcl::EarClipping::triangulate (const Vertices& vertices, PolygonMesh& output)
float
pcl::EarClipping::area (const std::vector<uint32_t>& vertices)
{
int n = static_cast<int> (vertices.size ());
float area = 0.0f;
Eigen::Vector2f prev_p, cur_p;
for (int prev = n - 1, cur = 0; cur < n; prev = cur++)
{
prev_p[0] = points_->points[vertices[prev]].x;
prev_p[1] = points_->points[vertices[prev]].y;
cur_p[0] = points_->points[vertices[cur]].x;
cur_p[1] = points_->points[vertices[cur]].y;
//if the polygon is projected onto the xy-plane, the area of the polygon is determined
//by the trapeze formula of Gauss. However this fails, if the projection is one 'line'.
//Therefore the following implementation determines the area of the flat polygon in 3D-space
//using Stoke's law: http://code.activestate.com/recipes/578276-3d-polygon-area/

int n = static_cast<int> (vertices.size ());
float area = 0.0f;
Eigen::Vector3f prev_p, cur_p;
Eigen::Vector3f total (0,0,0);
Eigen::Vector3f unit_normal;

if (n > 3)
{
for (int prev = n - 1, cur = 0; cur < n; prev = cur++)
{
prev_p = points_->points[vertices[prev]].getVector3fMap();
cur_p = points_->points[vertices[cur]].getVector3fMap();

area += crossProduct (prev_p, cur_p);
}
return (area * 0.5f);
total += prev_p.cross( cur_p );
}

//unit_normal is unit normal vector of plane defined by the first three points
prev_p = points_->points[vertices[1]].getVector3fMap() - points_->points[vertices[0]].getVector3fMap();
cur_p = points_->points[vertices[2]].getVector3fMap() - points_->points[vertices[0]].getVector3fMap();
unit_normal = (prev_p.cross(cur_p)).normalized();

area = total.dot( unit_normal );
}

return area * 0.5f;
}


/////////////////////////////////////////////////////////////////////////////////////////////
bool
pcl::EarClipping::isEar (int u, int v, int w, const std::vector<uint32_t>& vertices)
{
Eigen::Vector2f p_u, p_v, p_w;
p_u[0] = points_->points[vertices[u]].x;
p_u[1] = points_->points[vertices[u]].y;
p_v[0] = points_->points[vertices[v]].x;
p_v[1] = points_->points[vertices[v]].y;
p_w[0] = points_->points[vertices[w]].x;
p_w[1] = points_->points[vertices[w]].y;
Eigen::Vector3f p_u, p_v, p_w;
p_u = points_->points[vertices[u]].getVector3fMap();
p_v = points_->points[vertices[v]].getVector3fMap();
p_w = points_->points[vertices[w]].getVector3fMap();

// Avoid flat triangles.
// FIXME: triangulation would fail if all the triangles are flat in the X-Y axis
const float eps = 1e-15f;
Eigen::Vector2f p_uv, p_uw;
Eigen::Vector3f p_uv, p_uw;
p_uv = p_v - p_u;
p_uw = p_w - p_u;
if (crossProduct (p_uv, p_uw) < eps)

// Avoid flat triangles.
if ((p_uv.cross(p_uw)).norm() < eps)
return (false);

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

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

/////////////////////////////////////////////////////////////////////////////////////////////
bool
pcl::EarClipping::isInsideTriangle (const Eigen::Vector2f& u,
const Eigen::Vector2f& v,
const Eigen::Vector2f& w,
const Eigen::Vector2f& p)
pcl::EarClipping::isInsideTriangle (const Eigen::Vector3f& u,
const Eigen::Vector3f& v,
const Eigen::Vector3f& w,
const Eigen::Vector3f& p)
{
// Check first side.
if (crossProduct (w - v, p - v) < 0)
return (false);

// Check second side.
if (crossProduct (v - u, p - u) < 0)
return (false);

// Check third side.
if (crossProduct (u - w, p - w) < 0)
return (false);

return (true);
// see http://www.blackpawn.com/texts/pointinpoly/default.html
// Barycentric Coordinates
Eigen::Vector3f v0 = w - u;
Eigen::Vector3f v1 = v - u;
Eigen::Vector3f v2 = p - u;

// Compute dot products
float dot00 = v0.dot(v0);
float dot01 = v0.dot(v1);
float dot02 = v0.dot(v2);
float dot11 = v1.dot(v1);
float dot12 = v1.dot(v2);

// Compute barycentric coordinates
float invDenom = 1 / (dot00 * dot11 - dot01 * dot01);
float a = (dot11 * dot02 - dot01 * dot12) * invDenom;
float b = (dot00 * dot12 - dot01 * dot02) * invDenom;

// Check if point is in triangle
return (a >= 0) && (b >= 0) && (a + b < 1);
}

73 changes: 73 additions & 0 deletions test/surface/test_ear_clipping.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -106,6 +106,79 @@ TEST (PCL, EarClipping)
}
}

TEST (PCL, EarClippingCubeTest)
{
PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>());
cloud->height = 1;
//bottom of cube (z=0)
cloud->points.push_back (PointXYZ ( 0.f, 0.f, 0.f));
cloud->points.push_back (PointXYZ ( 1.f, 0.f, 0.f));
cloud->points.push_back (PointXYZ ( 1.f, 1.f, 0.f));
cloud->points.push_back (PointXYZ ( 0.f, 1.f, 0.f));
//top of cube (z=1.0)
cloud->points.push_back (PointXYZ ( 0.f, 0.f, 1.f));
cloud->points.push_back (PointXYZ ( 1.f, 0.f, 1.f));
cloud->points.push_back (PointXYZ ( 1.f, 1.f, 1.f));
cloud->points.push_back (PointXYZ ( 0.f, 1.f, 1.f));
cloud->width = static_cast<uint32_t> (cloud->points.size ());

Vertices vertices;
vertices.vertices.resize(4);

const int squares[][4] = { {1, 5, 6, 2},
{2, 6, 7, 3},
{3, 7, 4, 0},
{0, 4, 5, 1},
{4, 7, 6, 5},
{0, 1, 2, 3} };

const int truth[][3] = { {2, 1, 5},
{6, 2, 5},
{3, 2, 6},
{7, 3, 6},
{0, 3, 7},
{4, 0, 7},
{1, 0, 4},
{5, 1, 4},
{5, 4, 7},
{6, 5, 7},
{3, 0, 1},
{2, 3, 1} };

PolygonMesh::Ptr mesh (new PolygonMesh);
toPCLPointCloud2 (*cloud, mesh->cloud);

for (int i = 0; i < 6; ++i)
{
vertices.vertices[0] = squares[i][0];
vertices.vertices[1] = squares[i][1];
vertices.vertices[2] = squares[i][2];
vertices.vertices[3] = squares[i][3];
mesh->polygons.push_back (vertices);
}

EarClipping clipper;
PolygonMesh::ConstPtr mesh_aux (mesh);
clipper.setInputMesh (mesh_aux);

PolygonMesh triangulated_mesh;
clipper.process (triangulated_mesh);

EXPECT_EQ (triangulated_mesh.polygons.size (), 12);
for (int i = 0; i < static_cast<int> (triangulated_mesh.polygons.size ()); ++i)
EXPECT_EQ (triangulated_mesh.polygons[i].vertices.size (), 3);



for (int pi = 0; pi < static_cast<int> (triangulated_mesh.polygons.size ()); ++pi)
{
for (int vi = 0; vi < 3; ++vi)
{
EXPECT_EQ (triangulated_mesh.polygons[pi].vertices[vi], truth[pi][vi]);
}
}
}

/* ---[ */
int
main (int argc, char** argv)
Expand Down

0 comments on commit b30443e

Please sign in to comment.