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_RIFT_H_ 00042 #define PCL_FEATURES_IMPL_RIFT_H_ 00043 00044 #include <pcl/features/rift.h> 00045 00046 ////////////////////////////////////////////////////////////////////////////////////////////// 00047 template <typename PointInT, typename GradientT, typename PointOutT> void 00048 pcl::RIFTEstimation<PointInT, GradientT, PointOutT>::computeRIFT ( 00049 const PointCloudIn &cloud, const PointCloudGradient &gradient, 00050 int p_idx, float radius, const std::vector<int> &indices, 00051 const std::vector<float> &sqr_distances, Eigen::MatrixXf &rift_descriptor) 00052 { 00053 if (indices.empty ()) 00054 { 00055 PCL_ERROR ("[pcl::RIFTEstimation] Null indices points passed!\n"); 00056 return; 00057 } 00058 00059 // Determine the number of bins to use based on the size of rift_descriptor 00060 int nr_distance_bins = static_cast<int> (rift_descriptor.rows ()); 00061 int nr_gradient_bins = static_cast<int> (rift_descriptor.cols ()); 00062 00063 // Get the center point 00064 pcl::Vector3fMapConst p0 = cloud.points[p_idx].getVector3fMap (); 00065 00066 // Compute the RIFT descriptor 00067 rift_descriptor.setZero (); 00068 for (size_t idx = 0; idx < indices.size (); ++idx) 00069 { 00070 // Compute the gradient magnitude and orientation (relative to the center point) 00071 pcl::Vector3fMapConst point = cloud.points[indices[idx]].getVector3fMap (); 00072 Eigen::Map<const Eigen::Vector3f> gradient_vector (& (gradient.points[indices[idx]].gradient[0])); 00073 00074 float gradient_magnitude = gradient_vector.norm (); 00075 float gradient_angle_from_center = acosf (gradient_vector.dot ((point - p0).normalized ()) / gradient_magnitude); 00076 if (!pcl_isfinite (gradient_angle_from_center)) 00077 gradient_angle_from_center = 0.0; 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 = static_cast<float> (nr_distance_bins) * sqrtf (sqr_distances[idx]) / (radius + eps); 00082 float g = static_cast<float> (nr_gradient_bins) * gradient_angle_from_center / (static_cast<float> (M_PI) + eps); 00083 00084 // Compute the bin indices that need to be updated 00085 int d_idx_min = (std::max)(static_cast<int> (ceil (d - 1)), 0); 00086 int d_idx_max = (std::min)(static_cast<int> (floor (d + 1)), nr_distance_bins - 1); 00087 int g_idx_min = static_cast<int> (ceil (g - 1)); 00088 int g_idx_max = static_cast<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.0f - fabsf (d - static_cast<float> (d_idx))) * (1.0f - fabsf (g - static_cast<float> (g_idx))); 00100 00101 rift_descriptor (d_idx, g_idx_wrapped) += w * gradient_magnitude; 00102 } 00103 } 00104 } 00105 00106 // Normalize the RIFT descriptor to unit magnitude 00107 rift_descriptor.normalize (); 00108 } 00109 00110 00111 ////////////////////////////////////////////////////////////////////////////////////////////// 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] ", getClassName ().c_str ()); 00154 PCL_ERROR ("The number of points in the input dataset differs from the number of points in the gradient!\n"); 00155 output.width = output.height = 0; 00156 output.points.clear (); 00157 return; 00158 } 00159 00160 Eigen::MatrixXf rift_descriptor (nr_distance_bins_, nr_gradient_bins_); 00161 std::vector<int> nn_indices; 00162 std::vector<float> nn_dist_sqr; 00163 00164 // Iterating over the entire index vector 00165 for (size_t idx = 0; idx < indices_->size (); ++idx) 00166 { 00167 // Find neighbors within the search radius 00168 tree_->radiusSearch ((*indices_)[idx], search_radius_, nn_indices, nn_dist_sqr); 00169 00170 // Compute the RIFT descriptor 00171 computeRIFT (*surface_, *gradient_, (*indices_)[idx], static_cast<float> (search_radius_), nn_indices, nn_dist_sqr, rift_descriptor); 00172 00173 // Copy into the resultant cloud 00174 int bin = 0; 00175 for (int g_bin = 0; g_bin < rift_descriptor.cols (); ++g_bin) 00176 for (int d_bin = 0; d_bin < rift_descriptor.rows (); ++d_bin) 00177 output.points[idx].histogram[bin++] = rift_descriptor (d_bin, g_bin); 00178 } 00179 } 00180 00181 #define PCL_INSTANTIATE_RIFTEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::RIFTEstimation<T,NT,OutT>; 00182 00183 #endif // PCL_FEATURES_IMPL_RIFT_H_ 00184