|
Point Cloud Library (PCL)
1.6.0
|
|
Point Cloud Library (PCL)
1.6.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2009, 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: rsd.hpp 5026 2012-03-12 02:51:44Z rusu $ 00035 * 00036 */ 00037 00038 #ifndef PCL_FEATURES_IMPL_RSD_H_ 00039 #define PCL_FEATURES_IMPL_RSD_H_ 00040 00041 #include <cfloat> 00042 #include <pcl/features/rsd.h> 00043 00045 template <typename PointInT, typename PointNT, typename PointOutT> Eigen::MatrixXf 00046 pcl::computeRSD (boost::shared_ptr<const pcl::PointCloud<PointInT> > &surface, boost::shared_ptr<const pcl::PointCloud<PointNT> > &normals, 00047 const std::vector<int> &indices, double max_dist, 00048 int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram) 00049 { 00050 // Check if the full histogram has to be saved or not 00051 Eigen::MatrixXf histogram; 00052 if (compute_histogram) 00053 histogram = Eigen::MatrixXf::Zero (nr_subdiv, nr_subdiv); 00054 00055 // Check if enough points are provided or not 00056 if (indices.size () < 2) 00057 { 00058 radii.r_max = 0; 00059 radii.r_min = 0; 00060 return histogram; 00061 } 00062 00063 // Initialize minimum and maximum angle values in each distance bin 00064 std::vector<std::vector<double> > min_max_angle_by_dist (nr_subdiv); 00065 min_max_angle_by_dist[0].resize (2); 00066 min_max_angle_by_dist[0][0] = min_max_angle_by_dist[0][1] = 0.0; 00067 for (int di=1; di<nr_subdiv; di++) 00068 { 00069 min_max_angle_by_dist[di].resize (2); 00070 min_max_angle_by_dist[di][0] = +DBL_MAX; 00071 min_max_angle_by_dist[di][1] = -DBL_MAX; 00072 } 00073 00074 // Compute distance by normal angle distribution for points 00075 std::vector<int>::const_iterator i, begin (indices.begin()), end (indices.end()); 00076 for(i = begin+1; i != end; ++i) 00077 { 00078 // compute angle between the two lines going through normals (disregard orientation!) 00079 double cosine = normals->points[*i].normal[0] * normals->points[*begin].normal[0] + 00080 normals->points[*i].normal[1] * normals->points[*begin].normal[1] + 00081 normals->points[*i].normal[2] * normals->points[*begin].normal[2]; 00082 if (cosine > 1) cosine = 1; 00083 if (cosine < -1) cosine = -1; 00084 double angle = acos (cosine); 00085 if (angle > M_PI/2) angle = M_PI - angle; 00086 00087 // Compute point to point distance 00088 double dist = sqrt ((surface->points[*i].x - surface->points[*begin].x) * (surface->points[*i].x - surface->points[*begin].x) + 00089 (surface->points[*i].y - surface->points[*begin].y) * (surface->points[*i].y - surface->points[*begin].y) + 00090 (surface->points[*i].z - surface->points[*begin].z) * (surface->points[*i].z - surface->points[*begin].z)); 00091 00092 if (dist > max_dist) 00093 continue; 00094 00095 // compute bins and increase 00096 int bin_d = (int) floor (nr_subdiv * dist / max_dist); 00097 if (compute_histogram) 00098 { 00099 int bin_a = std::min (nr_subdiv-1, (int) floor (nr_subdiv * angle / (M_PI/2))); 00100 histogram(bin_a, bin_d)++; 00101 } 00102 00103 // update min-max values for distance bins 00104 if (min_max_angle_by_dist[bin_d][0] > angle) min_max_angle_by_dist[bin_d][0] = angle; 00105 if (min_max_angle_by_dist[bin_d][1] < angle) min_max_angle_by_dist[bin_d][1] = angle; 00106 } 00107 00108 // Estimate radius from min and max lines 00109 double Amint_Amin = 0, Amint_d = 0; 00110 double Amaxt_Amax = 0, Amaxt_d = 0; 00111 for (int di=0; di<nr_subdiv; di++) 00112 { 00113 // combute the members of A'*A*r = A'*D 00114 if (min_max_angle_by_dist[di][1] >= 0) 00115 { 00116 double p_min = min_max_angle_by_dist[di][0]; 00117 double p_max = min_max_angle_by_dist[di][1]; 00118 double f = (di+0.5)*max_dist/nr_subdiv; 00119 Amint_Amin += p_min * p_min; 00120 Amint_d += p_min * f; 00121 Amaxt_Amax += p_max * p_max; 00122 Amaxt_d += p_max * f; 00123 } 00124 } 00125 float min_radius = Amint_Amin == 0 ? plane_radius : std::min (Amint_d/Amint_Amin, plane_radius); 00126 float max_radius = Amaxt_Amax == 0 ? plane_radius : std::min (Amaxt_d/Amaxt_Amax, plane_radius); 00127 00128 // Small correction of the systematic error of the estimation (based on analysis with nr_subdiv_ = 5) 00129 min_radius *= 1.1; 00130 max_radius *= 0.9; 00131 if (min_radius < max_radius) 00132 { 00133 radii.r_min = min_radius; 00134 radii.r_max = max_radius; 00135 } 00136 else 00137 { 00138 radii.r_max = min_radius; 00139 radii.r_min = max_radius; 00140 } 00141 00142 return histogram; 00143 } 00144 00146 template <typename PointNT, typename PointOutT> Eigen::MatrixXf 00147 pcl::computeRSD (boost::shared_ptr<const pcl::PointCloud<PointNT> > &normals, 00148 const std::vector<int> &indices, const std::vector<float> &sqr_dists, double max_dist, 00149 int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram) 00150 { 00151 // Check if the full histogram has to be saved or not 00152 Eigen::MatrixXf histogram; 00153 if (compute_histogram) 00154 histogram = Eigen::MatrixXf::Zero (nr_subdiv, nr_subdiv); 00155 00156 // Check if enough points are provided or not 00157 if (indices.size () < 2) 00158 { 00159 radii.r_max = 0; 00160 radii.r_min = 0; 00161 return histogram; 00162 } 00163 00164 // Initialize minimum and maximum angle values in each distance bin 00165 std::vector<std::vector<double> > min_max_angle_by_dist (nr_subdiv); 00166 min_max_angle_by_dist[0].resize (2); 00167 min_max_angle_by_dist[0][0] = min_max_angle_by_dist[0][1] = 0.0; 00168 for (int di=1; di<nr_subdiv; di++) 00169 { 00170 min_max_angle_by_dist[di].resize (2); 00171 min_max_angle_by_dist[di][0] = +DBL_MAX; 00172 min_max_angle_by_dist[di][1] = -DBL_MAX; 00173 } 00174 00175 // Compute distance by normal angle distribution for points 00176 std::vector<int>::const_iterator i, begin (indices.begin()), end (indices.end()); 00177 for(i = begin+1; i != end; ++i) 00178 { 00179 // compute angle between the two lines going through normals (disregard orientation!) 00180 double cosine = normals->points[*i].normal[0] * normals->points[*begin].normal[0] + 00181 normals->points[*i].normal[1] * normals->points[*begin].normal[1] + 00182 normals->points[*i].normal[2] * normals->points[*begin].normal[2]; 00183 if (cosine > 1) cosine = 1; 00184 if (cosine < -1) cosine = -1; 00185 double angle = acos (cosine); 00186 if (angle > M_PI/2) angle = M_PI - angle; 00187 00188 // Compute point to point distance 00189 double dist = sqrt (sqr_dists[i-begin]); 00190 00191 if (dist > max_dist) 00192 continue; 00193 00194 // compute bins and increase 00195 int bin_d = (int) floor (nr_subdiv * dist / max_dist); 00196 if (compute_histogram) 00197 { 00198 int bin_a = std::min (nr_subdiv-1, (int) floor (nr_subdiv * angle / (M_PI/2))); 00199 histogram(bin_a, bin_d)++; 00200 } 00201 00202 // update min-max values for distance bins 00203 if (min_max_angle_by_dist[bin_d][0] > angle) min_max_angle_by_dist[bin_d][0] = angle; 00204 if (min_max_angle_by_dist[bin_d][1] < angle) min_max_angle_by_dist[bin_d][1] = angle; 00205 } 00206 00207 // Estimate radius from min and max lines 00208 double Amint_Amin = 0, Amint_d = 0; 00209 double Amaxt_Amax = 0, Amaxt_d = 0; 00210 for (int di=0; di<nr_subdiv; di++) 00211 { 00212 // combute the members of A'*A*r = A'*D 00213 if (min_max_angle_by_dist[di][1] >= 0) 00214 { 00215 double p_min = min_max_angle_by_dist[di][0]; 00216 double p_max = min_max_angle_by_dist[di][1]; 00217 double f = (di+0.5)*max_dist/nr_subdiv; 00218 Amint_Amin += p_min * p_min; 00219 Amint_d += p_min * f; 00220 Amaxt_Amax += p_max * p_max; 00221 Amaxt_d += p_max * f; 00222 } 00223 } 00224 float min_radius = Amint_Amin == 0 ? plane_radius : std::min (Amint_d/Amint_Amin, plane_radius); 00225 float max_radius = Amaxt_Amax == 0 ? plane_radius : std::min (Amaxt_d/Amaxt_Amax, plane_radius); 00226 00227 // Small correction of the systematic error of the estimation (based on analysis with nr_subdiv_ = 5) 00228 min_radius *= 1.1; 00229 max_radius *= 0.9; 00230 if (min_radius < max_radius) 00231 { 00232 radii.r_min = min_radius; 00233 radii.r_max = max_radius; 00234 } 00235 else 00236 { 00237 radii.r_max = min_radius; 00238 radii.r_min = max_radius; 00239 } 00240 00241 return histogram; 00242 } 00243 00245 template <typename PointInT, typename PointNT, typename PointOutT> void 00246 pcl::RSDEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output) 00247 { 00248 // Check if search_radius_ was set 00249 if (search_radius_ < 0) 00250 { 00251 PCL_ERROR ("[pcl::%s::computeFeature] A search radius needs to be set!\n", getClassName ().c_str ()); 00252 output.width = output.height = 0; 00253 output.points.clear (); 00254 return; 00255 } 00256 00257 // List of indices and corresponding squared distances for a neighborhood 00258 // \note resize is irrelevant for a radiusSearch (). 00259 std::vector<int> nn_indices; 00260 std::vector<float> nn_sqr_dists; 00261 00262 // Check if the full histogram has to be saved or not 00263 if (save_histograms_) 00264 { 00265 // Reserve space for the output histogram dataset 00266 histograms_.reset (new std::vector<Eigen::MatrixXf, Eigen::aligned_allocator<Eigen::MatrixXf> >); 00267 histograms_->reserve (output.points.size ()); 00268 00269 // Iterating over the entire index vector 00270 for (size_t idx = 0; idx < indices_->size (); ++idx) 00271 { 00272 // Compute and store r_min and r_max in the output cloud 00273 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_sqr_dists); 00274 //histograms_->push_back (computeRSD (surface_, normals_, nn_indices, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], true)); 00275 histograms_->push_back (computeRSD (normals_, nn_indices, nn_sqr_dists, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], true)); 00276 } 00277 } 00278 else 00279 { 00280 // Iterating over the entire index vector 00281 for (size_t idx = 0; idx < indices_->size (); ++idx) 00282 { 00283 // Compute and store r_min and r_max in the output cloud 00284 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_sqr_dists); 00285 //computeRSD (surface_, normals_, nn_indices, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], false); 00286 computeRSD (normals_, nn_indices, nn_sqr_dists, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], false); 00287 } 00288 } 00289 } 00290 00291 #define PCL_INSTANTIATE_RSDEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::RSDEstimation<T,NT,OutT>; 00292 00293 #endif // PCL_FEATURES_IMPL_VFH_H_
1.7.6.1
