ear_clipping_patched.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #include "jsk_recognition_utils/pcl/ear_clipping_patched.h"
00037 #include <pcl/surface/ear_clipping.h>
00038 #include <pcl/conversions.h>
00039 #include <pcl/pcl_config.h>
00040 
00042 bool
00043 pcl::EarClippingPatched::initCompute ()
00044 {
00045   points_.reset (new pcl::PointCloud<pcl::PointXYZ>);
00046 
00047   if (!MeshProcessing::initCompute ())
00048     return (false);
00049   fromPCLPointCloud2 (input_mesh_->cloud, *points_);
00050 
00051   return (true);
00052 }
00053 
00055 void
00056 pcl::EarClippingPatched::performProcessing (PolygonMesh& output)
00057 {
00058   output.polygons.clear ();
00059   output.cloud = input_mesh_->cloud;
00060   for (int i = 0; i < static_cast<int> (input_mesh_->polygons.size ()); ++i)
00061     triangulate (input_mesh_->polygons[i], output);
00062 }
00063 
00065 void
00066 pcl::EarClippingPatched::triangulate (const Vertices& vertices, PolygonMesh& output)
00067 {
00068   const int n_vertices = static_cast<const int> (vertices.vertices.size ());
00069 
00070   if (n_vertices < 3)
00071     return;
00072   else if (n_vertices == 3)
00073   {
00074     output.polygons.push_back( vertices );
00075     return;
00076   }
00077 
00078   std::vector<uint32_t> remaining_vertices (n_vertices);
00079   if (area (vertices.vertices) > 0) // clockwise?
00080     remaining_vertices = vertices.vertices;
00081   else
00082     for (int v = 0; v < n_vertices; v++)
00083       remaining_vertices[v] = vertices.vertices[n_vertices - 1 - v];
00084 
00085   // Avoid closed loops.
00086   if (remaining_vertices.front () == remaining_vertices.back ())
00087     remaining_vertices.erase (remaining_vertices.end () - 1);
00088 
00089   // null_iterations avoids infinite loops if the polygon is not simple.
00090   for (int u = static_cast<int> (remaining_vertices.size ()) - 1, null_iterations = 0;
00091       remaining_vertices.size () > 2 && null_iterations < static_cast<int >(remaining_vertices.size () * 2);
00092       ++null_iterations, u = (u+1) % static_cast<int> (remaining_vertices.size ()))
00093   {
00094     int v = (u + 1) % static_cast<int> (remaining_vertices.size ());
00095     int w = (u + 2) % static_cast<int> (remaining_vertices.size ());
00096 
00097     if (isEar (u, v, w, remaining_vertices))
00098     {
00099       Vertices triangle;
00100       triangle.vertices.resize (3);
00101       triangle.vertices[0] = remaining_vertices[u];
00102       triangle.vertices[1] = remaining_vertices[v];
00103       triangle.vertices[2] = remaining_vertices[w];
00104       output.polygons.push_back (triangle);
00105       remaining_vertices.erase (remaining_vertices.begin () + v);
00106       null_iterations = 0;
00107     }
00108   }
00109 }
00110 
00111 
00113 float
00114 pcl::EarClippingPatched::area (const std::vector<uint32_t>& vertices)
00115 {
00116     //if the polygon is projected onto the xy-plane, the area of the polygon is determined
00117     //by the trapeze formula of Gauss. However this fails, if the projection is one 'line'.
00118     //Therefore the following implementation determines the area of the flat polygon in 3D-space
00119     //using Stoke's law: http://code.activestate.com/recipes/578276-3d-polygon-area/
00120 
00121     int n = static_cast<int> (vertices.size ());
00122     float area = 0.0f;
00123     Eigen::Vector3f prev_p, cur_p;
00124     Eigen::Vector3f total (0,0,0);
00125     Eigen::Vector3f unit_normal;
00126 
00127     if (n > 3)
00128     {
00129         for (int prev = n - 1, cur = 0; cur < n; prev = cur++)
00130         {
00131             prev_p = points_->points[vertices[prev]].getVector3fMap();
00132             cur_p = points_->points[vertices[cur]].getVector3fMap();
00133 
00134             total += prev_p.cross( cur_p );
00135         }
00136 
00137         //unit_normal is unit normal vector of plane defined by the first three points
00138         prev_p = points_->points[vertices[1]].getVector3fMap() - points_->points[vertices[0]].getVector3fMap();
00139         cur_p = points_->points[vertices[2]].getVector3fMap() - points_->points[vertices[0]].getVector3fMap();
00140         unit_normal = (prev_p.cross(cur_p)).normalized();
00141 
00142         area = total.dot( unit_normal );
00143     }
00144 
00145     return area * 0.5f; 
00146 }
00147 
00148 
00150 bool
00151 pcl::EarClippingPatched::isEar (int u, int v, int w, const std::vector<uint32_t>& vertices)
00152 {
00153   Eigen::Vector3f p_u, p_v, p_w;
00154   p_u = points_->points[vertices[u]].getVector3fMap();
00155   p_v = points_->points[vertices[v]].getVector3fMap();
00156   p_w = points_->points[vertices[w]].getVector3fMap();
00157 
00158   const float eps = 1e-15f;
00159   Eigen::Vector3f p_uv, p_uw;
00160   p_uv = p_v - p_u;
00161   p_uw = p_w - p_u;
00162 
00163   // Avoid flat triangles.
00164   if ((p_uv.cross(p_uw)).norm() < eps)
00165     return (false);
00166 
00167   Eigen::Vector3f p;
00168   // Check if any other vertex is inside the triangle.
00169   for (int k = 0; k < static_cast<int> (vertices.size ()); k++)
00170   {
00171     if ((k == u) || (k == v) || (k == w))
00172       continue;
00173     p = points_->points[vertices[k]].getVector3fMap();
00174 
00175     if (isInsideTriangle (p_u, p_v, p_w, p))
00176       return (false);
00177   }
00178   return (true);
00179 }
00180 
00182 bool
00183 pcl::EarClippingPatched::isInsideTriangle (const Eigen::Vector3f& u,
00184                                     const Eigen::Vector3f& v,
00185                                     const Eigen::Vector3f& w,
00186                                     const Eigen::Vector3f& p)
00187 {
00188   // see http://www.blackpawn.com/texts/pointinpoly/default.html
00189   // Barycentric Coordinates
00190   Eigen::Vector3f v0 = w - u;
00191   Eigen::Vector3f v1 = v - u;
00192   Eigen::Vector3f v2 = p - u;
00193 
00194   // Compute dot products
00195   float dot00 = v0.dot(v0);
00196   float dot01 = v0.dot(v1);
00197   float dot02 = v0.dot(v2);
00198   float dot11 = v1.dot(v1);
00199   float dot12 = v1.dot(v2);
00200 
00201   // Compute barycentric coordinates
00202   float invDenom = 1 / (dot00 * dot11 - dot01 * dot01);
00203   float a = (dot11 * dot02 - dot01 * dot12) * invDenom;
00204   float b = (dot00 * dot12 - dot01 * dot02) * invDenom;
00205 
00206   // Check if point is in triangle
00207   return (a >= 0) && (b >= 0) && (a + b < 1);
00208 }
00209 


jsk_recognition_utils
Author(s):
autogenerated on Sun Oct 8 2017 02:42:48