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