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_VFH_H_ 00042 #define PCL_FEATURES_IMPL_VFH_H_ 00043 00044 #include <pcl/features/vfh.h> 00045 #include <pcl/features/pfh_tools.h> 00046 #include <pcl/common/common.h> 00047 #include <pcl/common/centroid.h> 00048 00049 ////////////////////////////////////////////////////////////////////////////////////////////// 00050 template<typename PointInT, typename PointNT, typename PointOutT> bool 00051 pcl::VFHEstimation<PointInT, PointNT, PointOutT>::initCompute () 00052 { 00053 if (input_->points.size () < 2 || (surface_ && surface_->points.size () < 2)) 00054 { 00055 PCL_ERROR ("[pcl::VFHEstimation::initCompute] Input dataset must have at least 2 points!\n"); 00056 return (false); 00057 } 00058 if (search_radius_ == 0 && k_ == 0) 00059 k_ = 1; 00060 return (Feature<PointInT, PointOutT>::initCompute ()); 00061 } 00062 00063 ////////////////////////////////////////////////////////////////////////////////////////////// 00064 template<typename PointInT, typename PointNT, typename PointOutT> void 00065 pcl::VFHEstimation<PointInT, PointNT, PointOutT>::compute (PointCloudOut &output) 00066 { 00067 if (!initCompute ()) 00068 { 00069 output.width = output.height = 0; 00070 output.points.clear (); 00071 return; 00072 } 00073 // Copy the header 00074 output.header = input_->header; 00075 00076 // Resize the output dataset 00077 // Important! We should only allocate precisely how many elements we will need, otherwise 00078 // we risk at pre-allocating too much memory which could lead to bad_alloc 00079 // (see http://dev.pointclouds.org/issues/657) 00080 output.width = output.height = 1; 00081 output.is_dense = input_->is_dense; 00082 output.points.resize (1); 00083 00084 // Perform the actual feature computation 00085 computeFeature (output); 00086 00087 Feature<PointInT, PointOutT>::deinitCompute (); 00088 } 00089 00090 ////////////////////////////////////////////////////////////////////////////////////////////// 00091 template<typename PointInT, typename PointNT, typename PointOutT> void 00092 pcl::VFHEstimation<PointInT, PointNT, PointOutT>::computePointSPFHSignature (const Eigen::Vector4f ¢roid_p, 00093 const Eigen::Vector4f ¢roid_n, 00094 const pcl::PointCloud<PointInT> &cloud, 00095 const pcl::PointCloud<PointNT> &normals, 00096 const std::vector<int> &indices) 00097 { 00098 Eigen::Vector4f pfh_tuple; 00099 // Reset the whole thing 00100 hist_f1_.setZero (nr_bins_f1_); 00101 hist_f2_.setZero (nr_bins_f2_); 00102 hist_f3_.setZero (nr_bins_f3_); 00103 hist_f4_.setZero (nr_bins_f4_); 00104 00105 // Get the bounding box of the current cluster 00106 //Eigen::Vector4f min_pt, max_pt; 00107 //pcl::getMinMax3D (cloud, indices, min_pt, max_pt); 00108 //double distance_normalization_factor = (std::max)((centroid_p - min_pt).norm (), (centroid_p - max_pt).norm ()); 00109 00110 //Instead of using the bounding box to normalize the VFH distance component, it is better to use the max_distance 00111 //from any point to centroid. VFH is invariant to rotation about the roll axis but the bounding box is not, 00112 //resulting in different normalization factors for point clouds that are just rotated about that axis. 00113 00114 double distance_normalization_factor = 1.0; 00115 if (normalize_distances_) 00116 { 00117 Eigen::Vector4f max_pt; 00118 pcl::getMaxDistance (cloud, indices, centroid_p, max_pt); 00119 max_pt[3] = 0; 00120 distance_normalization_factor = (centroid_p - max_pt).norm (); 00121 } 00122 00123 // Factorization constant 00124 float hist_incr; 00125 if (normalize_bins_) 00126 hist_incr = 100.0f / static_cast<float> (indices.size () - 1); 00127 else 00128 hist_incr = 1.0f; 00129 00130 float hist_incr_size_component; 00131 if (size_component_) 00132 hist_incr_size_component = hist_incr; 00133 else 00134 hist_incr_size_component = 0.0; 00135 00136 // Iterate over all the points in the neighborhood 00137 for (size_t idx = 0; idx < indices.size (); ++idx) 00138 { 00139 // Compute the pair P to NNi 00140 if (!computePairFeatures (centroid_p, centroid_n, cloud.points[indices[idx]].getVector4fMap (), 00141 normals.points[indices[idx]].getNormalVector4fMap (), pfh_tuple[0], pfh_tuple[1], 00142 pfh_tuple[2], pfh_tuple[3])) 00143 continue; 00144 00145 // Normalize the f1, f2, f3, f4 features and push them in the histogram 00146 int h_index = static_cast<int> (floor (nr_bins_f1_ * ((pfh_tuple[0] + M_PI) * d_pi_))); 00147 if (h_index < 0) 00148 h_index = 0; 00149 if (h_index >= nr_bins_f1_) 00150 h_index = nr_bins_f1_ - 1; 00151 hist_f1_ (h_index) += hist_incr; 00152 00153 h_index = static_cast<int> (floor (nr_bins_f2_ * ((pfh_tuple[1] + 1.0) * 0.5))); 00154 if (h_index < 0) 00155 h_index = 0; 00156 if (h_index >= nr_bins_f2_) 00157 h_index = nr_bins_f2_ - 1; 00158 hist_f2_ (h_index) += hist_incr; 00159 00160 h_index = static_cast<int> (floor (nr_bins_f3_ * ((pfh_tuple[2] + 1.0) * 0.5))); 00161 if (h_index < 0) 00162 h_index = 0; 00163 if (h_index >= nr_bins_f3_) 00164 h_index = nr_bins_f3_ - 1; 00165 hist_f3_ (h_index) += hist_incr; 00166 00167 if (normalize_distances_) 00168 h_index = static_cast<int> (floor (nr_bins_f4_ * (pfh_tuple[3] / distance_normalization_factor))); 00169 else 00170 h_index = static_cast<int> (pcl_round (pfh_tuple[3] * 100)); 00171 00172 if (h_index < 0) 00173 h_index = 0; 00174 if (h_index >= nr_bins_f4_) 00175 h_index = nr_bins_f4_ - 1; 00176 00177 hist_f4_ (h_index) += hist_incr_size_component; 00178 } 00179 } 00180 ////////////////////////////////////////////////////////////////////////////////////////////// 00181 template <typename PointInT, typename PointNT, typename PointOutT> void 00182 pcl::VFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output) 00183 { 00184 // ---[ Step 1a : compute the centroid in XYZ space 00185 Eigen::Vector4f xyz_centroid; 00186 00187 if (use_given_centroid_) 00188 xyz_centroid = centroid_to_use_; 00189 else 00190 compute3DCentroid (*surface_, *indices_, xyz_centroid); // Estimate the XYZ centroid 00191 00192 // ---[ Step 1b : compute the centroid in normal space 00193 Eigen::Vector4f normal_centroid = Eigen::Vector4f::Zero (); 00194 int cp = 0; 00195 00196 // If the data is dense, we don't need to check for NaN 00197 if (use_given_normal_) 00198 normal_centroid = normal_to_use_; 00199 else 00200 { 00201 if (normals_->is_dense) 00202 { 00203 for (size_t i = 0; i < indices_->size (); ++i) 00204 { 00205 normal_centroid += normals_->points[(*indices_)[i]].getNormalVector4fMap (); 00206 cp++; 00207 } 00208 } 00209 // NaN or Inf values could exist => check for them 00210 else 00211 { 00212 for (size_t i = 0; i < indices_->size (); ++i) 00213 { 00214 if (!pcl_isfinite (normals_->points[(*indices_)[i]].normal[0]) 00215 || 00216 !pcl_isfinite (normals_->points[(*indices_)[i]].normal[1]) 00217 || 00218 !pcl_isfinite (normals_->points[(*indices_)[i]].normal[2])) 00219 continue; 00220 normal_centroid += normals_->points[(*indices_)[i]].getNormalVector4fMap (); 00221 cp++; 00222 } 00223 } 00224 normal_centroid /= static_cast<float> (cp); 00225 } 00226 00227 // Compute the direction of view from the viewpoint to the centroid 00228 Eigen::Vector4f viewpoint (vpx_, vpy_, vpz_, 0); 00229 Eigen::Vector4f d_vp_p = viewpoint - xyz_centroid; 00230 d_vp_p.normalize (); 00231 00232 // Estimate the SPFH at nn_indices[0] using the entire cloud 00233 computePointSPFHSignature (xyz_centroid, normal_centroid, *surface_, *normals_, *indices_); 00234 00235 // We only output _1_ signature 00236 output.points.resize (1); 00237 output.width = 1; 00238 output.height = 1; 00239 00240 // Estimate the FPFH at nn_indices[0] using the entire cloud and copy the resultant signature 00241 for (int d = 0; d < hist_f1_.size (); ++d) 00242 output.points[0].histogram[d + 0] = hist_f1_[d]; 00243 00244 size_t data_size = hist_f1_.size (); 00245 for (int d = 0; d < hist_f2_.size (); ++d) 00246 output.points[0].histogram[d + data_size] = hist_f2_[d]; 00247 00248 data_size += hist_f2_.size (); 00249 for (int d = 0; d < hist_f3_.size (); ++d) 00250 output.points[0].histogram[d + data_size] = hist_f3_[d]; 00251 00252 data_size += hist_f3_.size (); 00253 for (int d = 0; d < hist_f4_.size (); ++d) 00254 output.points[0].histogram[d + data_size] = hist_f4_[d]; 00255 00256 // ---[ Step 2 : obtain the viewpoint component 00257 hist_vp_.setZero (nr_bins_vp_); 00258 00259 double hist_incr; 00260 if (normalize_bins_) 00261 hist_incr = 100.0 / static_cast<double> (indices_->size ()); 00262 else 00263 hist_incr = 1.0; 00264 00265 for (size_t i = 0; i < indices_->size (); ++i) 00266 { 00267 Eigen::Vector4f normal (normals_->points[(*indices_)[i]].normal[0], 00268 normals_->points[(*indices_)[i]].normal[1], 00269 normals_->points[(*indices_)[i]].normal[2], 0); 00270 // Normalize 00271 double alpha = (normal.dot (d_vp_p) + 1.0) * 0.5; 00272 int fi = static_cast<int> (floor (alpha * static_cast<double> (hist_vp_.size ()))); 00273 if (fi < 0) 00274 fi = 0; 00275 if (fi > (static_cast<int> (hist_vp_.size ()) - 1)) 00276 fi = static_cast<int> (hist_vp_.size ()) - 1; 00277 // Bin into the histogram 00278 hist_vp_ [fi] += static_cast<float> (hist_incr); 00279 } 00280 data_size += hist_f4_.size (); 00281 // Copy the resultant signature 00282 for (int d = 0; d < hist_vp_.size (); ++d) 00283 output.points[0].histogram[d + data_size] = hist_vp_[d]; 00284 } 00285 00286 #define PCL_INSTANTIATE_VFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::VFHEstimation<T,NT,OutT>; 00287 00288 #endif // PCL_FEATURES_IMPL_VFH_H_