ear_clipping.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: ear_clipping.cpp 5027 2012-03-12 03:10:45Z rusu $
00035  *
00036  */
00037 
00038 #include <pcl/surface/ear_clipping.h>
00039 #include <pcl/ros/conversions.h>
00040 #include <pcl/pcl_config.h>
00041 
00043 bool
00044 pcl::EarClipping::initCompute ()
00045 {
00046   points_.reset (new pcl::PointCloud<pcl::PointXYZ>);
00047 
00048   if (!MeshProcessing::initCompute ())
00049     return (false);
00050   fromROSMsg (input_mesh_->cloud, *points_);
00051 
00052   return (true);
00053 }
00054 
00056 void
00057 pcl::EarClipping::performProcessing (PolygonMesh& output)
00058 {
00059   output.polygons.clear ();
00060   output.cloud = input_mesh_->cloud;
00061   for (int i = 0; i < static_cast<int> (input_mesh_->polygons.size ()); ++i)
00062     triangulate (input_mesh_->polygons[i], output);
00063 }
00064 
00066 void
00067 pcl::EarClipping::triangulate (const Vertices& vertices, PolygonMesh& output)
00068 {
00069   const int n_vertices = static_cast<const int> (vertices.vertices.size ());
00070 
00071   if (n_vertices <= 3)
00072     return;
00073 
00074   std::vector<uint32_t> remaining_vertices (n_vertices);
00075   if (area (vertices.vertices) > 0) // clockwise?
00076     remaining_vertices = vertices.vertices;
00077   else
00078     for (int v = 0; v < n_vertices; v++)
00079       remaining_vertices[v] = vertices.vertices[n_vertices - 1 - v];
00080 
00081   // Avoid closed loops.
00082   if (remaining_vertices.front () == remaining_vertices.back ())
00083     remaining_vertices.erase (remaining_vertices.end () - 1);
00084 
00085   // null_iterations avoids infinite loops if the polygon is not simple.
00086   for (int u = static_cast<int> (remaining_vertices.size ()) - 1, null_iterations = 0;
00087       remaining_vertices.size () > 2 && null_iterations < static_cast<int >(remaining_vertices.size () * 2);
00088       ++null_iterations, u = (u+1) % static_cast<int> (remaining_vertices.size ()))
00089   {
00090     int v = (u + 1) % static_cast<int> (remaining_vertices.size ());
00091     int w = (u + 2) % static_cast<int> (remaining_vertices.size ());
00092 
00093     if (isEar (u, v, w, remaining_vertices))
00094     {
00095       Vertices triangle;
00096       triangle.vertices.resize (3);
00097       triangle.vertices[0] = remaining_vertices[u];
00098       triangle.vertices[1] = remaining_vertices[v];
00099       triangle.vertices[2] = remaining_vertices[w];
00100       output.polygons.push_back (triangle);
00101       remaining_vertices.erase (remaining_vertices.begin () + v);
00102       null_iterations = 0;
00103     }
00104   }
00105 }
00106 
00107 
00109 float
00110 pcl::EarClipping::area (const std::vector<uint32_t>& vertices)
00111 {
00112   int n = static_cast<int> (vertices.size ());
00113   float area = 0.0f;
00114   Eigen::Vector2f prev_p, cur_p;
00115   for (int prev = n - 1, cur = 0; cur < n; prev = cur++)
00116   {
00117     prev_p[0] = points_->points[vertices[prev]].x;
00118     prev_p[1] = points_->points[vertices[prev]].y;
00119     cur_p[0] = points_->points[vertices[cur]].x;
00120     cur_p[1] = points_->points[vertices[cur]].y;
00121 
00122     area += crossProduct (prev_p, cur_p);
00123   }
00124   return (area * 0.5f);
00125 }
00126 
00127 
00129 bool
00130 pcl::EarClipping::isEar (int u, int v, int w, const std::vector<uint32_t>& vertices)
00131 {
00132   Eigen::Vector2f p_u, p_v, p_w;
00133   p_u[0] = points_->points[vertices[u]].x;
00134   p_u[1] = points_->points[vertices[u]].y;
00135   p_v[0] = points_->points[vertices[v]].x;
00136   p_v[1] = points_->points[vertices[v]].y;
00137   p_w[0] = points_->points[vertices[w]].x;
00138   p_w[1] = points_->points[vertices[w]].y;
00139 
00140   // Avoid flat triangles.
00141   // FIXME: triangulation would fail if all the triangles are flat in the X-Y axis
00142   const float eps = 1e-15f;
00143   Eigen::Vector2f p_uv, p_uw;
00144   p_uv = p_v - p_u;
00145   p_uw = p_w - p_u;
00146   if (crossProduct (p_uv, p_uw) < eps)
00147     return (false);
00148 
00149   Eigen::Vector2f p;
00150   // Check if any other vertex is inside the triangle.
00151   for (int k = 0; k < static_cast<int> (vertices.size ()); k++)
00152   {
00153     if ((k == u) || (k == v) || (k == w))
00154       continue;
00155     p[0] = points_->points[vertices[k]].x;
00156     p[1] = points_->points[vertices[k]].y;
00157 
00158     if (isInsideTriangle (p_u, p_v, p_w, p))
00159       return (false);
00160   }
00161   return (true);
00162 }
00163 
00165 bool
00166 pcl::EarClipping::isInsideTriangle (const Eigen::Vector2f& u,
00167                                     const Eigen::Vector2f& v,
00168                                     const Eigen::Vector2f& w,
00169                                     const Eigen::Vector2f& p)
00170 {
00171   // Check first side.
00172   if (crossProduct (w - v, p - v) < 0)
00173     return (false);
00174 
00175   // Check second side.
00176   if (crossProduct (v - u, p - u) < 0)
00177     return (false);
00178 
00179   // Check third side.
00180   if (crossProduct (u - w, p - w) < 0)
00181     return (false);
00182 
00183   return (true);
00184 }
00185 


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:14:49