Point Cloud Library (PCL)
1.7.0
|
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 00041 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 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 00069 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 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 / sqrtf (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> ().matrix () = 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_