Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
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)
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
00082 if (remaining_vertices.front () == remaining_vertices.back ())
00083 remaining_vertices.erase (remaining_vertices.end () - 1);
00084
00085
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
00141
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
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
00172 if (crossProduct (w - v, p - v) < 0)
00173 return (false);
00174
00175
00176 if (crossProduct (v - u, p - u) < 0)
00177 return (false);
00178
00179
00180 if (crossProduct (u - w, p - w) < 0)
00181 return (false);
00182
00183 return (true);
00184 }
00185