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