Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2011, Willow Garage, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of Willow Garage, Inc. nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * $Id: boundary.hpp 2497 2011-09-18 06:33:45Z rusu $ 00037 * 00038 */ 00039 00040 #ifndef PCL_FEATURES_IMPL_BOUNDARY_H_ 00041 #define PCL_FEATURES_IMPL_BOUNDARY_H_ 00042 00043 #include "pcl/features/boundary.h" 00044 #include <cfloat> 00045 00047 template <typename PointInT, typename PointNT, typename PointOutT> bool 00048 pcl::BoundaryEstimation<PointInT, PointNT, PointOutT>::isBoundaryPoint ( 00049 const pcl::PointCloud<PointInT> &cloud, int q_idx, 00050 const std::vector<int> &indices, 00051 const Eigen::Vector4f &u, const Eigen::Vector4f &v, 00052 const float angle_threshold) 00053 { 00054 return (isBoundaryPoint (cloud, cloud.points[q_idx], indices, u, v, angle_threshold)); 00055 } 00056 00058 template <typename PointInT, typename PointNT, typename PointOutT> bool 00059 pcl::BoundaryEstimation<PointInT, PointNT, PointOutT>::isBoundaryPoint ( 00060 const pcl::PointCloud<PointInT> &cloud, const PointInT &q_point, 00061 const std::vector<int> &indices, 00062 const Eigen::Vector4f &u, const Eigen::Vector4f &v, 00063 const float angle_threshold) 00064 { 00065 if (indices.size () < 3) 00066 return (false); 00067 00068 if (!pcl_isfinite (q_point.x) || !pcl_isfinite (q_point.y) || !pcl_isfinite (q_point.z)) 00069 return (false); 00070 00071 // Compute the angles between each neighboring point and the query point itself 00072 std::vector<float> angles (indices.size ()); 00073 float max_dif = FLT_MIN, dif; 00074 int cp = 0; 00075 00076 for (size_t i = 0; i < indices.size (); ++i) 00077 { 00078 if (!pcl_isfinite (cloud.points[indices[i]].x) || 00079 !pcl_isfinite (cloud.points[indices[i]].y) || 00080 !pcl_isfinite (cloud.points[indices[i]].z)) 00081 continue; 00082 00083 Eigen::Vector4f delta = cloud.points[indices[i]].getVector4fMap () - q_point.getVector4fMap (); 00084 00085 angles[cp++] = atan2f (v.dot (delta), u.dot (delta)); // the angles are fine between -PI and PI too 00086 } 00087 if (cp == 0) 00088 return (false); 00089 00090 angles.resize (cp); 00091 std::sort (angles.begin (), angles.end ()); 00092 00093 // Compute the maximal angle difference between two consecutive angles 00094 for (size_t i = 0; i < angles.size () - 1; ++i) 00095 { 00096 dif = angles[i + 1] - angles[i]; 00097 if (max_dif < dif) 00098 max_dif = dif; 00099 } 00100 // Get the angle difference between the last and the first 00101 dif = 2 * M_PI - angles[angles.size () - 1] + angles[0]; 00102 if (max_dif < dif) 00103 max_dif = dif; 00104 00105 // Check results 00106 if (max_dif > angle_threshold) 00107 return (true); 00108 else 00109 return (false); 00110 } 00111 00113 template <typename PointInT, typename PointNT, typename PointOutT> void 00114 pcl::BoundaryEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output) 00115 { 00116 // Allocate enough space to hold the results 00117 // \note This resize is irrelevant for a radiusSearch (). 00118 std::vector<int> nn_indices (k_); 00119 std::vector<float> nn_dists (k_); 00120 00121 Eigen::Vector4f u = Eigen::Vector4f::Zero (), v = Eigen::Vector4f::Zero (); 00122 00123 // Iterating over the entire index vector 00124 for (size_t idx = 0; idx < indices_->size (); ++idx) 00125 { 00126 if (!pcl_isfinite (input_->points[(*indices_)[idx]].x) || 00127 !pcl_isfinite (input_->points[(*indices_)[idx]].y) || 00128 !pcl_isfinite (input_->points[(*indices_)[idx]].z)) 00129 { 00130 output.points[idx].boundary_point = 0; 00131 continue; 00132 } 00133 00134 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists); 00135 00136 // Obtain a coordinate system on the least-squares plane 00137 //v = normals_->points[(*indices_)[idx]].getNormalVector4fMap ().unitOrthogonal (); 00138 //u = normals_->points[(*indices_)[idx]].getNormalVector4fMap ().cross3 (v); 00139 getCoordinateSystemOnPlane (normals_->points[(*indices_)[idx]], u, v); 00140 00141 // Estimate whether the point is lying on a boundary surface or not 00142 output.points[idx].boundary_point = isBoundaryPoint (*surface_, input_->points[(*indices_)[idx]], nn_indices, u, v, angle_threshold_); 00143 } 00144 } 00145 00146 #define PCL_INSTANTIATE_BoundaryEstimation(PointInT,PointNT,PointOutT) template class PCL_EXPORTS pcl::BoundaryEstimation<PointInT, PointNT, PointOutT>; 00147 00148 #endif // PCL_FEATURES_IMPL_BOUNDARY_H_ 00149