polygon_operations.hpp
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  */
00035 
00036 #ifndef PCL_GEOMETRY_POLYGON_OPERATIONS_HPP_
00037 #define PCL_GEOMETRY_POLYGON_OPERATIONS_HPP_
00038 
00039 #include <pcl/geometry/polygon_operations.h>
00040 
00042 template<typename PointT> void
00043 pcl::approximatePolygon (const PlanarPolygon<PointT>& polygon, PlanarPolygon<PointT>& approx_polygon, float threshold, bool refine, bool closed)
00044 {
00045   const Eigen::Vector4f& coefficients = polygon.getCoefficients ();
00046   const typename pcl::PointCloud<PointT>::VectorType &contour = polygon.getContour ();
00047   
00048   Eigen::Vector3f rotation_axis (coefficients[1], -coefficients[0], 0.0f);
00049   rotation_axis.normalize ();
00050 
00051   float rotation_angle = acosf (coefficients [2]);
00052   Eigen::Affine3f transformation = Eigen::Translation3f (0, 0, coefficients [3]) * Eigen::AngleAxisf (rotation_angle, rotation_axis);
00053 
00054   typename pcl::PointCloud<PointT>::VectorType polygon2D (contour.size ());
00055   for (unsigned pIdx = 0; pIdx < polygon2D.size (); ++pIdx)
00056     polygon2D [pIdx].getVector3fMap () = transformation * contour [pIdx].getVector3fMap ();
00057 
00058   typename pcl::PointCloud<PointT>::VectorType approx_polygon2D;
00059   approximatePolygon2D<PointT> (polygon2D, approx_polygon2D, threshold, refine, closed);
00060   
00061   typename pcl::PointCloud<PointT>::VectorType &approx_contour = approx_polygon.getContour ();
00062   approx_contour.resize (approx_polygon2D.size ());
00063   
00064   Eigen::Affine3f inv_transformation = transformation.inverse ();
00065   for (unsigned pIdx = 0; pIdx < approx_polygon2D.size (); ++pIdx)
00066     approx_contour [pIdx].getVector3fMap () = inv_transformation * approx_polygon2D [pIdx].getVector3fMap ();
00067 }
00068 
00070 template <typename PointT> void
00071 pcl::approximatePolygon2D (const typename pcl::PointCloud<PointT>::VectorType &polygon, 
00072                            typename pcl::PointCloud<PointT>::VectorType &approx_polygon, 
00073                            float threshold, bool refine, bool closed)
00074 {
00075   approx_polygon.clear ();
00076   if (polygon.size () < 3)
00077     return;
00078   
00079   std::vector<std::pair<unsigned, unsigned> > intervals;
00080   std::pair<unsigned,unsigned> interval (0, 0);
00081   
00082   if (closed)
00083   {
00084     float max_distance = .0f;
00085     for (unsigned idx = 1; idx < polygon.size (); ++idx)
00086     {
00087       float distance = (polygon [0].x - polygon [idx].x) * (polygon [0].x - polygon [idx].x) + 
00088                        (polygon [0].y - polygon [idx].y) * (polygon [0].y - polygon [idx].y);
00089 
00090       if (distance > max_distance)
00091       {
00092         max_distance = distance;
00093         interval.second = idx;
00094       }
00095     }
00096 
00097     for (unsigned idx = 1; idx < polygon.size (); ++idx)
00098     {
00099       float distance = (polygon [interval.second].x - polygon [idx].x) * (polygon [interval.second].x - polygon [idx].x) + 
00100                        (polygon [interval.second].y - polygon [idx].y) * (polygon [interval.second].y - polygon [idx].y);
00101 
00102       if (distance > max_distance)
00103       {
00104         max_distance = distance;
00105         interval.first = idx;
00106       }
00107     }
00108 
00109     if (max_distance < threshold * threshold)
00110       return;
00111 
00112     intervals.push_back (interval);
00113     std::swap (interval.first, interval.second);
00114     intervals.push_back (interval);
00115   }
00116   else
00117   {
00118     interval.first = 0;
00119     interval.second = static_cast<unsigned int> (polygon.size ()) - 1;
00120     intervals.push_back (interval);
00121   }
00122   
00123   std::vector<unsigned> result;
00124   // recursively refine
00125   while (!intervals.empty ())
00126   {
00127     std::pair<unsigned, unsigned>& currentInterval = intervals.back ();
00128     float line_x = polygon [currentInterval.first].y - polygon [currentInterval.second].y;
00129     float line_y = polygon [currentInterval.second].x - polygon [currentInterval.first].x;
00130     float line_d = polygon [currentInterval.first].x * polygon [currentInterval.second].y - polygon [currentInterval.first].y * polygon [currentInterval.second].x;
00131     
00132     float linelen = 1.0f / sqrt (line_x * line_x + line_y * line_y);
00133     
00134     line_x *= linelen;
00135     line_y *= linelen;
00136     line_d *= linelen;
00137     
00138     float max_distance = 0.0;
00139     unsigned first_index = currentInterval.first + 1;
00140     unsigned max_index = 0;
00141 
00142     // => 0-crossing
00143     if (currentInterval.first > currentInterval.second)
00144     {
00145       for (unsigned idx = first_index; idx < polygon.size(); idx++)
00146       {
00147         float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
00148         if (distance > max_distance)
00149         {
00150           max_distance = distance;
00151           max_index  = idx;
00152         }
00153       }
00154       first_index = 0;
00155     }
00156 
00157     for (unsigned int idx = first_index; idx < currentInterval.second; idx++)
00158     {
00159       float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
00160       if (distance > max_distance)
00161       {
00162         max_distance = distance;
00163         max_index  = idx;
00164       }
00165     }
00166 
00167     if (max_distance > threshold)
00168     {
00169       std::pair<unsigned, unsigned> interval (max_index, currentInterval.second);
00170       currentInterval.second = max_index;
00171       intervals.push_back (interval);
00172     }
00173     else
00174     {
00175       result.push_back (currentInterval.second);
00176       intervals.pop_back ();
00177     }
00178   }
00179   
00180   approx_polygon.reserve (result.size ());
00181   if (refine)
00182   {
00183     std::vector<Eigen::Vector3f> lines (result.size ());
00184     std::reverse (result.begin (), result.end ());
00185     for (unsigned rIdx = 0; rIdx < result.size (); ++rIdx)
00186     {
00187       unsigned nIdx = rIdx + 1;
00188       if (nIdx == result.size ())
00189         nIdx = 0;
00190       
00191       Eigen::Vector2f centroid = Eigen::Vector2f::Zero ();
00192       Eigen::Matrix2f covariance = Eigen::Matrix2f::Zero ();
00193       unsigned pIdx = result[rIdx];
00194       unsigned num_points = 0;
00195       if (pIdx > result[nIdx])
00196       {
00197         num_points = static_cast<unsigned> (polygon.size ()) - pIdx;
00198         for (; pIdx < polygon.size (); ++pIdx)
00199         {
00200           covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x;
00201           covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y;
00202           covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y;
00203           centroid [0] += polygon [pIdx].x;
00204           centroid [1] += polygon [pIdx].y;
00205         }
00206         pIdx = 0;
00207       }
00208       
00209       num_points += result[nIdx] - pIdx;
00210       for (; pIdx < result[nIdx]; ++pIdx)
00211       {
00212         covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x;
00213         covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y;
00214         covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y;
00215         centroid [0] += polygon [pIdx].x;
00216         centroid [1] += polygon [pIdx].y;
00217       }
00218       
00219       covariance.coeffRef (2) = covariance.coeff (1);
00220       
00221       float norm = 1.0f / float (num_points);
00222       centroid *= norm;
00223       covariance *= norm;
00224       covariance.coeffRef (0) -= centroid [0] * centroid [0];
00225       covariance.coeffRef (1) -= centroid [0] * centroid [1];
00226       covariance.coeffRef (3) -= centroid [1] * centroid [1];
00227       
00228       float eval;
00229       Eigen::Vector2f normal;
00230       eigen22 (covariance, eval, normal);
00231 
00232       // select the one which is more "parallel" to the original line
00233       Eigen::Vector2f direction;
00234       direction [0] = polygon[result[nIdx]].x - polygon[result[rIdx]].x;
00235       direction [1] = polygon[result[nIdx]].y - polygon[result[rIdx]].y;
00236       direction.normalize ();
00237       
00238       if (fabs (direction.dot (normal)) > float(M_SQRT1_2))
00239       {
00240         std::swap (normal [0], normal [1]);
00241         normal [0] = -normal [0];
00242       }
00243       
00244       // needs to be on the left side of the edge
00245       if (direction [0] * normal [1] < direction [1] * normal [0])
00246         normal *= -1.0;
00247       
00248       lines [rIdx].head<2> () = normal;
00249       lines [rIdx] [2] = -normal.dot (centroid);
00250     }
00251     
00252     float threshold2 = threshold * threshold;
00253     for (unsigned rIdx = 0; rIdx < lines.size (); ++rIdx)
00254     {
00255       unsigned nIdx = rIdx + 1;
00256       if (nIdx == result.size ())
00257         nIdx = 0;      
00258       
00259       Eigen::Vector3f vertex = lines [rIdx].cross (lines [nIdx]);
00260       vertex /= vertex [2];
00261       vertex [2] = 0.0;
00262 
00263       PointT point;      
00264       // test whether we need another edge since the intersection point is too far away from the original vertex
00265       Eigen::Vector3f pq = polygon [result[nIdx]].getVector3fMap () - vertex;
00266       pq [2] = 0.0;
00267       
00268       float distance = pq.squaredNorm ();
00269       if (distance > threshold2)
00270       {
00271         // test whether the old point is inside the new polygon or outside
00272         if ((pq [0] * lines [rIdx] [0] + pq [1] * lines [rIdx] [1] < 0.0) &&
00273             (pq [0] * lines [nIdx] [0] + pq [1] * lines [nIdx] [1] < 0.0) )
00274         {
00275           float distance1 = lines [rIdx] [0] * polygon[result[nIdx]].x + lines [rIdx] [1] * polygon[result[nIdx]].y + lines [rIdx] [2];
00276           float distance2 = lines [nIdx] [0] * polygon[result[nIdx]].x + lines [nIdx] [1] * polygon[result[nIdx]].y + lines [nIdx] [2];
00277 
00278           point.x = polygon[result[nIdx]].x - distance1 * lines [rIdx] [0];
00279           point.y = polygon[result[nIdx]].y - distance1 * lines [rIdx] [1];
00280 
00281           approx_polygon.push_back (point);
00282 
00283           vertex [0] = polygon[result[nIdx]].x - distance2 * lines [nIdx] [0];
00284           vertex [1] = polygon[result[nIdx]].y - distance2 * lines [nIdx] [1];
00285         }
00286       }
00287       point.getVector3fMap () = vertex;
00288       approx_polygon.push_back (point);
00289     }
00290   }
00291   else
00292   {
00293     // we have a new polygon in results, but inverted (clockwise <-> counter-clockwise)    
00294     for (std::vector<unsigned>::reverse_iterator it = result.rbegin (); it != result.rend (); ++it)
00295       approx_polygon.push_back (polygon [*it]);
00296   }
00297 }
00298 
00299 #endif // PCL_GEOMETRY_POLYGON_OPERATIONS_HPP_


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:17:21