Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, www.pointclouds.org 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: usc.hpp 3275 2011-11-30 16:59:04Z mdixon $ 00035 * 00036 */ 00037 00038 #ifndef PCL_FEATURES_IMPL_USC_HPP_ 00039 #define PCL_FEATURES_IMPL_USC_HPP_ 00040 00041 #include <pcl/features/usc.h> 00042 #include <pcl/features/shot_common.h> 00043 #include <pcl/common/geometry.h> 00044 #include <pcl/common/angles.h> 00045 00046 template <typename PointInT, typename PointOutT> bool 00047 pcl::UniqueShapeContext<PointInT, PointOutT>::initCompute() 00048 { 00049 if (!Feature<PointInT, PointOutT>::initCompute ()) 00050 { 00051 PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); 00052 return (false); 00053 } 00054 00055 if( search_radius_< min_radius_) 00056 { 00057 PCL_ERROR ("[pcl::%s::initCompute] search_radius_ must be GREATER than min_radius_.\n", getClassName ().c_str ()); 00058 return (false); 00059 } 00060 00061 // Update descriptor length 00062 descriptor_length_ = elevation_bins_ * azimuth_bins_ * radius_bins_; 00063 00064 // Compute radial, elevation and azimuth divisions 00065 float azimuth_interval = (float) 360 / azimuth_bins_; 00066 float elevation_interval = (float) 180 / elevation_bins_; 00067 00068 // Reallocate divisions and volume lut 00069 radii_interval_.clear (); 00070 phi_divisions_.clear (); 00071 theta_divisions_.clear (); 00072 volume_lut_.clear (); 00073 00074 // Fills radii interval based on formula (1) in section 2.1 of Frome's paper 00075 radii_interval_.resize(radius_bins_+1); 00076 for(size_t j = 0; j < radius_bins_+1; j++) 00077 radii_interval_[j] = exp (log (min_radius_) + (((float) j / (radius_bins_)) * log (search_radius_/min_radius_))); 00078 // Fill theta didvisions of elevation 00079 theta_divisions_.resize (elevation_bins_+1); 00080 for(size_t k = 0; k < elevation_bins_+1; k++) 00081 theta_divisions_[k] = k*elevation_interval; 00082 // Fill phi didvisions of elevation 00083 phi_divisions_.resize (azimuth_bins_+1); 00084 for(size_t l = 0; l < azimuth_bins_+1; l++) 00085 phi_divisions_[l] = l*azimuth_interval; 00086 00087 // LookUp Table that contains the volume of all the bins 00088 // "phi" term of the volume integral 00089 // "integr_phi" has always the same value so we compute it only one time 00090 float integr_phi = pcl::deg2rad(phi_divisions_[1]) - pcl::deg2rad(phi_divisions_[0]); 00091 // exponential to compute the cube root using pow 00092 float e = 1.0 / 3.0; 00093 // Resize volume look up table 00094 volume_lut_.resize (radius_bins_*elevation_bins_*azimuth_bins_); 00095 // Fill volumes look up table 00096 for(size_t j = 0; j < radius_bins_; j++) 00097 { 00098 // "r" term of the volume integral 00099 float integr_r = (radii_interval_[j+1]*radii_interval_[j+1]*radii_interval_[j+1] / 3) - (radii_interval_[j]*radii_interval_[j]*radii_interval_[j]/ 3); 00100 00101 for(size_t k = 0; k < elevation_bins_; k++) 00102 { 00103 // "theta" term of the volume integral 00104 float integr_theta = cos (deg2rad (theta_divisions_[k])) - cos (deg2rad (theta_divisions_[k+1])); 00105 // Volume 00106 float V = integr_phi * integr_theta * integr_r; 00107 // Compute cube root of the computed volume commented for performance but left 00108 // here for clarity 00109 // float cbrt = pow(V, e); 00110 // cbrt = 1 / cbrt; 00111 00112 for(size_t l = 0; l < azimuth_bins_; l++) 00113 { 00114 // Store in lut 1/cbrt 00115 //volume_lut_[ (l*elevation_bins_*radius_bins_) + k*radius_bins_ + j ] = cbrt; 00116 volume_lut_[ (l*elevation_bins_*radius_bins_) + k*radius_bins_ + j ] = 1 / pow(V,e); 00117 } 00118 } 00119 } 00120 return (true); 00121 } 00122 00123 template <typename PointInT, typename PointOutT> void 00124 pcl::UniqueShapeContext<PointInT, PointOutT>::computePointRF(size_t index, float rf[9]) 00125 { 00126 std::vector<int> nn_indices; 00127 std::vector<float> nn_dists; 00128 size_t nb_neighbours = searchForNeighbors ((*indices_)[index], local_radius_, nn_indices, nn_dists); 00129 std::cout << "nb of neighbors " << nb_neighbours << std::endl; 00131 if (nb_neighbours < 5) 00132 { 00133 PCL_WARN ("[pcl::%s::computePointRF] Neighborhood has %d vertices which is less than 5, aborting description of point index %d\n!", getClassName ().c_str (), nb_neighbours, index); 00134 return; 00135 } 00136 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > rf_(3); 00137 Eigen::Vector4f central_point = input_->points[(*indices_)[index]].getVector4fMap (); 00138 central_point[3] = 0; 00139 pcl::getLocalRF(*surface_, local_radius_, central_point /*(*indices_)[index]*/, nn_indices, nn_dists, rf_); 00140 for (int d = 0; d < 9; ++d) 00141 rf[d] = rf_[d/3][d%3]; 00142 } 00143 00144 template <typename PointInT, typename PointOutT> void 00145 pcl::UniqueShapeContext<PointInT, PointOutT>::computePointDescriptor(size_t index, float rf[9], std::vector<float> &desc) 00146 { 00147 pcl::Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap (); 00148 const Eigen::Map<Eigen::Vector3f> x_axis (rf); 00149 const Eigen::Map<Eigen::Vector3f> y_axis (rf + 3); 00150 const Eigen::Map<Eigen::Vector3f> normal (rf + 6); 00152 std::vector<int> nn_indices; 00153 std::vector<float> nn_dists; 00154 const size_t neighb_cnt = searchForNeighbors (index, search_radius_, nn_indices, nn_dists); 00156 for(size_t ne = 0; ne < neighb_cnt; ne++) 00157 { 00158 if(nn_indices[ne] == (*indices_)[index]) 00159 continue; 00161 Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap (); 00162 00164 00166 float r = sqrt (nn_dists[ne]); 00167 00169 Eigen::Vector3f proj; 00170 pcl::geometry::project(neighbour, origin, normal, proj); 00171 proj-= origin; 00172 00174 proj.normalize (); 00175 00177 Eigen::Vector3f cross = x_axis.cross(proj); 00178 float phi = rad2deg (std::atan2(cross.norm (), x_axis.dot (proj))); 00179 phi = cross.dot (normal) < 0.f ? (360.0 - phi) : phi; 00181 Eigen::Vector3f no = neighbour - origin; 00182 no.normalize (); 00183 float theta = normal.dot (no); 00184 theta = rad2deg (acos(std::min(1.0f, std::max( -1.0f, theta)))); 00185 00187 size_t j = 0; 00188 size_t k = 0; 00189 size_t l = 0; 00190 00192 for(size_t rad = 1; rad < radius_bins_+1; rad++) { 00193 if(r <= radii_interval_[rad]) { 00194 j = rad-1; 00195 break; 00196 } 00197 } 00198 00199 for(size_t ang = 1; ang < elevation_bins_+1; ang++) { 00200 if(theta <= theta_divisions_[ang]) { 00201 k = ang-1; 00202 break; 00203 } 00204 } 00205 00206 for(size_t ang = 1; ang < azimuth_bins_+1; ang++) { 00207 if(phi <= phi_divisions_[ang]) { 00208 l = ang-1; 00209 break; 00210 } 00211 } 00212 00214 std::vector<int> neighbour_indices; 00215 std::vector<float> neighbour_didtances; 00216 float point_density = (float) searchForNeighbors (nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_didtances); 00218 float w = (1.0 / point_density) * volume_lut_[ (l*elevation_bins_*radius_bins_) + 00219 (k*radius_bins_) + 00220 j ]; 00221 00222 assert(w >= 0.0); 00223 if(w == std::numeric_limits<float>::infinity()) 00224 PCL_ERROR("Shape Context Error INF!\n"); 00225 if(w != w) 00226 PCL_ERROR("Shape Context Error IND!\n"); 00228 desc[ (l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j ] += w; 00229 00230 assert(desc[ (l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j ] >= 0); 00231 } // end for each neighbour 00232 } 00233 00234 template <typename PointInT, typename PointOutT> void 00235 pcl::UniqueShapeContext<PointInT, PointOutT>::computeFeature (PointCloudOut &output) 00236 { 00237 for(size_t point_index = 0; point_index < indices_->size (); point_index++) 00238 { 00239 output[point_index].descriptor.resize (descriptor_length_); 00240 computePointRF(point_index, output[point_index].rf); 00241 computePointDescriptor(point_index, output[point_index].rf, output[point_index].descriptor); 00242 } 00243 } 00244 00245 #define PCL_INSTANTIATE_UniqueShapeContext(T,OutT) template class PCL_EXPORTS pcl::UniqueShapeContext<T,OutT>; 00246 00247 #endif