Point Cloud Library (PCL)
1.3.1
|
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 * $Id: intensity_spin.hpp 1370 2011-06-19 01:06:01Z jspricke $ 00035 * 00036 */ 00037 00038 #ifndef PCL_FEATURES_IMPL_INTENSITY_SPIN_H_ 00039 #define PCL_FEATURES_IMPL_INTENSITY_SPIN_H_ 00040 00041 #include "pcl/features/intensity_spin.h" 00042 00044 template <typename PointInT, typename PointOutT> void 00045 pcl::IntensitySpinEstimation<PointInT, PointOutT>::computeIntensitySpinImage ( 00046 const PointCloudIn &cloud, float radius, float sigma, 00047 int k, 00048 const std::vector<int> &indices, 00049 const std::vector<float> &squared_distances, Eigen::MatrixXf &intensity_spin_image) 00050 { 00051 // Determine the number of bins to use based on the size of intensity_spin_image 00052 int nr_distance_bins = intensity_spin_image.cols (); 00053 int nr_intensity_bins = intensity_spin_image.rows (); 00054 00055 // Find the min and max intensity values in the given neighborhood 00056 float min_intensity = std::numeric_limits<float>::max (); 00057 float max_intensity = std::numeric_limits<float>::min (); 00058 for (int idx = 0; idx < k; ++idx) 00059 { 00060 min_intensity = (std::min) (min_intensity, cloud.points[indices[idx]].intensity); 00061 max_intensity = (std::max) (max_intensity, cloud.points[indices[idx]].intensity); 00062 } 00063 00064 // Compute the intensity spin image 00065 intensity_spin_image.setZero (); 00066 for (int idx = 0; idx < k; ++idx) 00067 { 00068 // Normalize distance and intensity values to: 0.0 <= d,i < nr_distance_bins,nr_intensity_bins 00069 const float eps = std::numeric_limits<float>::epsilon (); 00070 float d = nr_distance_bins * sqrt (squared_distances[idx]) / (radius + eps); 00071 float i = nr_intensity_bins * 00072 (cloud.points[indices[idx]].intensity - min_intensity) / (max_intensity - min_intensity + eps); 00073 00074 if (sigma == 0) 00075 { 00076 // If sigma is zero, update the histogram with no smoothing kernel 00077 int d_idx = (int) d; 00078 int i_idx = (int) i; 00079 intensity_spin_image (i_idx, d_idx) += 1; 00080 } 00081 else 00082 { 00083 // Compute the bin indices that need to be updated (+/- 3 standard deviations) 00084 int d_idx_min = (std::max)((int) floor (d - 3*sigma), 0); 00085 int d_idx_max = (std::min)((int) ceil (d + 3*sigma), nr_distance_bins - 1); 00086 int i_idx_min = (std::max)((int) floor (i - 3*sigma), 0); 00087 int i_idx_max = (std::min)((int) ceil (i + 3*sigma), nr_intensity_bins - 1); 00088 00089 // Update the appropriate bins of the histogram 00090 for (int i_idx = i_idx_min; i_idx <= i_idx_max; ++i_idx) 00091 { 00092 for (int d_idx = d_idx_min; d_idx <= d_idx_max; ++d_idx) 00093 { 00094 // Compute a "soft" update weight based on the distance between the point and the bin 00095 float w = exp (-pow (d - d_idx, 2) / (2.0*sigma_*sigma_) 00096 -pow (i - i_idx, 2) / (2.0*sigma_*sigma_)); 00097 intensity_spin_image (i_idx, d_idx) += w; 00098 } 00099 } 00100 } 00101 } 00102 } 00103 00105 template <typename PointInT, typename PointOutT> void 00106 pcl::IntensitySpinEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output) 00107 { 00108 // Make sure a search radius is set 00109 if (search_radius_ == 0.0) 00110 { 00111 PCL_ERROR ("[pcl::%s::computeFeature] The search radius must be set before computing the feature!\n", 00112 getClassName ().c_str ()); 00113 output.width = output.height = 0; 00114 output.points.clear (); 00115 return; 00116 } 00117 00118 // Make sure the spin image has valid dimensions 00119 if (nr_intensity_bins_ <= 0) 00120 { 00121 PCL_ERROR ("[pcl::%s::computeFeature] The number of intensity bins must be greater than zero!\n", 00122 getClassName ().c_str ()); 00123 output.width = output.height = 0; 00124 output.points.clear (); 00125 return; 00126 } 00127 if (nr_distance_bins_ <= 0) 00128 { 00129 PCL_ERROR ("[pcl::%s::computeFeature] The number of distance bins must be greater than zero!\n", 00130 getClassName ().c_str ()); 00131 output.width = output.height = 0; 00132 output.points.clear (); 00133 return; 00134 } 00135 00136 Eigen::MatrixXf intensity_spin_image (nr_intensity_bins_, nr_distance_bins_); 00137 // Allocate enough space to hold the radiusSearch results 00138 std::vector<int> nn_indices (surface_->points.size ()); 00139 std::vector<float> nn_dist_sqr (surface_->points.size ()); 00140 00141 // Iterating over the entire index vector 00142 for (size_t idx = 0; idx < indices_->size (); ++idx) 00143 { 00144 // Find neighbors within the search radius 00145 // TODO: do we want to use searchForNeigbors instead? 00146 int k = tree_->radiusSearch ((*indices_)[idx], search_radius_, nn_indices, nn_dist_sqr); 00147 00148 // Compute the intensity spin image 00149 computeIntensitySpinImage (*surface_, search_radius_, sigma_, k, nn_indices, nn_dist_sqr, intensity_spin_image); 00150 00151 // Copy into the resultant cloud 00152 for (int bin = 0; bin < intensity_spin_image.size (); ++bin) 00153 output.points[idx].histogram[bin] = intensity_spin_image (bin); 00154 } 00155 } 00156 00157 #define PCL_INSTANTIATE_IntensitySpinEstimation(T,NT) template class PCL_EXPORTS pcl::IntensitySpinEstimation<T,NT>; 00158 00159 #endif // PCL_FEATURES_IMPL_INTENSITY_SPIN_H_ 00160