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: rift.hpp 1370 2011-06-19 01:06:01Z jspricke $ 00035 * 00036 */ 00037 00038 #ifndef PCL_FEATURES_IMPL_RIFT_H_ 00039 #define PCL_FEATURES_IMPL_RIFT_H_ 00040 00041 #include "pcl/features/rift.h" 00042 00044 template <typename PointInT, typename GradientT, typename PointOutT> void 00045 pcl::RIFTEstimation<PointInT, GradientT, PointOutT>::computeRIFT ( 00046 const PointCloudIn &cloud, const PointCloudGradient &gradient, 00047 int p_idx, float radius, const std::vector<int> &indices, 00048 const std::vector<float> &sqr_distances, Eigen::MatrixXf &rift_descriptor) 00049 { 00050 if (indices.empty ()) 00051 { 00052 PCL_ERROR ("[pcl::RIFTEstimation] Null indices points passed!\n"); 00053 return; 00054 } 00055 00056 // Determine the number of bins to use based on the size of rift_descriptor 00057 int nr_distance_bins = rift_descriptor.cols (); 00058 int nr_gradient_bins = rift_descriptor.rows (); 00059 00060 // Get the center point 00061 pcl::Vector3fMapConst p0 = cloud.points[p_idx].getVector3fMap (); 00062 00063 // Compute the RIFT descriptor 00064 rift_descriptor.setZero (); 00065 for (size_t idx = 0; idx < indices.size (); ++idx) 00066 { 00067 // Compute the gradient magnitude and orientation (relative to the center point) 00069 pcl::Vector3fMapConst point = cloud.points[indices[idx]].getVector3fMap (); 00070 Eigen::Map<const Eigen::Vector3f> gradient_vector (& (gradient.points[indices[idx]].gradient[0])); 00071 00072 float gradient_magnitude = gradient_vector.norm (); 00073 float gradient_angle_from_center = acos (gradient_vector.dot ((point - p0).normalized ()) / gradient_magnitude); 00074 if (!pcl_isfinite (gradient_angle_from_center)) 00075 { 00076 gradient_angle_from_center = 0.0; 00077 } 00078 00079 // Normalize distance and angle values to: 0.0 <= d,g < nr_distances_bins,nr_gradient_bins 00080 const float eps = std::numeric_limits<float>::epsilon (); 00081 float d = nr_distance_bins * sqrt (sqr_distances[idx]) / (radius + eps); 00082 float g = nr_gradient_bins * gradient_angle_from_center / (M_PI + eps); 00083 00084 // Compute the bin indices that need to be updated 00085 int d_idx_min = (std::max)((int) ceil (d - 1), 0); 00086 int d_idx_max = (std::min)((int) floor (d + 1), nr_distance_bins - 1); 00087 int g_idx_min = (int) ceil (g - 1); 00088 int g_idx_max = (int) floor (g + 1); 00089 00090 // Update the appropriate bins of the histogram 00091 for (int g_idx = g_idx_min; g_idx <= g_idx_max; ++g_idx) 00092 { 00093 // Because gradient orientation is cyclical, out-of-bounds values must wrap around 00094 int g_idx_wrapped = ((g_idx + nr_gradient_bins) % nr_gradient_bins); 00095 00096 for (int d_idx = d_idx_min; d_idx <= d_idx_max; ++d_idx) 00097 { 00098 // To avoid boundary effects, use linear interpolation when updating each bin 00099 float w = (1 - fabs (d - d_idx)) * (1 - fabs (g - g_idx)); 00100 00101 rift_descriptor (g_idx_wrapped * nr_distance_bins + d_idx) += w * gradient_magnitude; 00102 } 00103 } 00104 } 00105 00106 // Normalize the RIFT descriptor to unit magnitude 00107 rift_descriptor.normalize (); 00108 } 00109 00110 00112 template <typename PointInT, typename GradientT, typename PointOutT> void 00113 pcl::RIFTEstimation<PointInT, GradientT, PointOutT>::computeFeature (PointCloudOut &output) 00114 { 00115 // Make sure a search radius is set 00116 if (search_radius_ == 0.0) 00117 { 00118 PCL_ERROR ("[pcl::%s::computeFeature] The search radius must be set before computing the feature!\n", 00119 getClassName ().c_str ()); 00120 output.width = output.height = 0; 00121 output.points.clear (); 00122 return; 00123 } 00124 00125 // Make sure the RIFT descriptor has valid dimensions 00126 if (nr_gradient_bins_ <= 0) 00127 { 00128 PCL_ERROR ("[pcl::%s::computeFeature] The number of gradient bins must be greater than zero!\n", 00129 getClassName ().c_str ()); 00130 output.width = output.height = 0; 00131 output.points.clear (); 00132 return; 00133 } 00134 if (nr_distance_bins_ <= 0) 00135 { 00136 PCL_ERROR ("[pcl::%s::computeFeature] The number of distance bins must be greater than zero!\n", 00137 getClassName ().c_str ()); 00138 output.width = output.height = 0; 00139 output.points.clear (); 00140 return; 00141 } 00142 00143 // Check for valid input gradient 00144 if (!gradient_) 00145 { 00146 PCL_ERROR ("[pcl::%s::computeFeature] No input gradient was given!\n", getClassName ().c_str ()); 00147 output.width = output.height = 0; 00148 output.points.clear (); 00149 return; 00150 } 00151 if (gradient_->points.size () != surface_->points.size ()) 00152 { 00153 PCL_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the gradient!\n", getClassName ().c_str ()); 00154 output.width = output.height = 0; 00155 output.points.clear (); 00156 return; 00157 } 00158 00159 Eigen::MatrixXf rift_descriptor (nr_gradient_bins_, nr_distance_bins_); 00160 std::vector<int> nn_indices; 00161 std::vector<float> nn_dist_sqr; 00162 00163 // Iterating over the entire index vector 00164 for (size_t idx = 0; idx < indices_->size (); ++idx) 00165 { 00166 // Find neighbors within the search radius 00167 tree_->radiusSearch ((*indices_)[idx], search_radius_, nn_indices, nn_dist_sqr); 00168 00169 // Compute the RIFT descriptor 00170 computeRIFT (*surface_, *gradient_, (*indices_)[idx], search_radius_, nn_indices, nn_dist_sqr, rift_descriptor); 00171 00172 // Copy into the resultant cloud 00173 for (int bin = 0; bin < rift_descriptor.size (); ++bin) 00174 output.points[idx].histogram[bin] = rift_descriptor (bin); 00175 } 00176 } 00177 00178 #define PCL_INSTANTIATE_RIFTEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::RIFTEstimation<T,NT,OutT>; 00179 00180 #endif // PCL_FEATURES_IMPL_RIFT_H_ 00181