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: 3dsc.hpp 3023 2011-11-01 03:42:32Z svn $ 00035 * 00036 */ 00037 00038 #ifndef PCL_FEATURES_IMPL_3DSC_HPP_ 00039 #define PCL_FEATURES_IMPL_3DSC_HPP_ 00040 00041 #include <pcl/features/3dsc.h> 00042 #include <pcl/common/utils.h> 00043 #include <pcl/common/geometry.h> 00044 #include <pcl/common/angles.h> 00045 00046 template <typename PointInT, typename PointNT, typename PointOutT> bool 00047 pcl::ShapeContext3DEstimation<PointInT, PointNT, PointOutT>::initCompute() 00048 { 00049 if (!FeatureFromNormals<PointInT, PointNT, 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 PointNT, typename PointOutT> void 00124 pcl::ShapeContext3DEstimation<PointInT, PointNT, PointOutT>::computePoint(size_t index, const pcl::PointCloud<PointInT> &input, const pcl::PointCloud<PointNT> &normals, float rf[9], std::vector<float> &desc) 00125 { 00127 Eigen::Map<Eigen::Vector3f> x_axis (rf); 00128 Eigen::Map<Eigen::Vector3f> y_axis (rf + 3); 00129 Eigen::Map<Eigen::Vector3f> normal (rf + 6); 00130 00132 Vector3fMapConst origin = input[(*indices_)[index]].getVector3fMap (); 00135 normal = normals[(*indices_)[index]].getNormalVector3fMap (); 00136 00138 if(!pcl::utils::equal(normal[2], 0.0f)) 00139 { 00140 x_axis[0] = (float)rand() / ((float)RAND_MAX + 1); 00141 x_axis[1] = (float)rand() / ((float)RAND_MAX + 1); 00142 x_axis[2] = - (normal[0]*x_axis[0] + normal[1]*x_axis[1]) / normal[2]; 00143 } 00144 else if(!pcl::utils::equal(normal[1], 0.0f)) 00145 { 00146 x_axis[0] = (float)rand() / ((float)RAND_MAX + 1); 00147 x_axis[2] = (float)rand() / ((float)RAND_MAX + 1); 00148 x_axis[1] = - (normal[0]*x_axis[0] + normal[2]*x_axis[2]) / normal[1]; 00149 } 00150 else if(!pcl::utils::equal(normal[0], 0.0f)) 00151 { 00152 x_axis[1] = (float)rand() / ((float)RAND_MAX + 1); 00153 x_axis[2] = (float)rand() / ((float)RAND_MAX + 1); 00154 x_axis[0] = - (normal[1]*x_axis[1] + normal[2]*x_axis[2]) / normal[0]; 00155 } 00156 00157 x_axis.normalize(); 00158 00160 assert(pcl::utils::equal(x_axis[0]*normal[0] + x_axis[1]*normal[1] + x_axis[2]*normal[2], 0.0f, 1E-6f)); 00161 00163 y_axis = normal.cross (x_axis); 00165 std::vector<int> nn_indices; 00166 std::vector<float> nn_dists; 00167 const size_t neighb_cnt = searchForNeighbors (index, search_radius_, nn_indices, nn_dists); 00169 for(size_t ne = 0; ne < neighb_cnt; ne++) 00170 { 00171 if(nn_indices[ne] == (*indices_)[index]) 00172 continue; 00174 Eigen::Vector3f neighbour = input[nn_indices[ne]].getVector3fMap (); 00175 00177 00179 float r = sqrt (nn_dists[ne]); 00180 00182 Eigen::Vector3f proj; 00183 pcl::geometry::project(neighbour, origin, normal, proj); 00184 proj-= origin; 00185 00187 proj.normalize (); 00188 00190 Eigen::Vector3f cross = x_axis.cross(proj); 00191 float phi = rad2deg (std::atan2(cross.norm (), x_axis.dot (proj))); 00192 phi = cross.dot (normal) < 0.f ? (360.0 - phi) : phi; 00194 Eigen::Vector3f no = neighbour - origin; 00195 no.normalize (); 00196 float theta = normal.dot (no); 00197 theta = rad2deg (acos(std::min(1.0f, std::max( -1.0f, theta)))); 00198 00200 size_t j = 0; 00201 size_t k = 0; 00202 size_t l = 0; 00203 00205 for(size_t rad = 1; rad < radius_bins_+1; rad++) { 00206 if(r <= radii_interval_[rad]) { 00207 j = rad-1; 00208 break; 00209 } 00210 } 00211 00212 for(size_t ang = 1; ang < elevation_bins_+1; ang++) { 00213 if(theta <= theta_divisions_[ang]) { 00214 k = ang-1; 00215 break; 00216 } 00217 } 00218 00219 for(size_t ang = 1; ang < azimuth_bins_+1; ang++) { 00220 if(phi <= phi_divisions_[ang]) { 00221 l = ang-1; 00222 break; 00223 } 00224 } 00225 00227 std::vector<int> neighbour_indices; 00228 std::vector<float> neighbour_didtances; 00229 float point_density = (float) searchForNeighbors (nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_didtances); 00231 float w = (1.0 / point_density) * volume_lut_[ (l*elevation_bins_*radius_bins_) + 00232 (k*radius_bins_) + 00233 j ]; 00234 00235 assert(w >= 0.0); 00236 if(w == std::numeric_limits<float>::infinity()) 00237 PCL_ERROR("Shape Context Error INF!\n"); 00238 if(w != w) 00239 PCL_ERROR("Shape Context Error IND!\n"); 00241 desc[ (l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j ] += w; 00242 00243 assert(desc[ (l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j ] >= 0); 00244 } // end for each neighbour 00245 } 00246 00247 template <typename PointInT, typename PointNT, typename PointOutT> void 00248 pcl::ShapeContext3DEstimation<PointInT, PointNT, PointOutT>::shiftAlongAzimuth(size_t block_size, std::vector<float>& desc) 00249 { 00250 assert(desc.size () == descriptor_length_); 00251 // L rotations for each descriptor 00252 desc.resize(descriptor_length_ * azimuth_bins_); 00253 // Create L azimuth rotated descriptors from reference descriptor 00254 // The descriptor_length_ first ones are the same so start at 1 00255 for(size_t l = 1; l < azimuth_bins_; l++) 00256 for(size_t bin = 0; bin < descriptor_length_; bin++) 00257 desc[(l * descriptor_length_) + bin] = desc[(l*block_size + bin) % descriptor_length_]; 00258 } 00259 00260 template <typename PointInT, typename PointNT, typename PointOutT> void 00261 pcl::ShapeContext3DEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output) 00262 { 00263 if(!shift_) 00264 { 00265 for(size_t point_index = 0; point_index < indices_->size (); point_index++) 00266 { 00267 output[point_index].descriptor.resize (descriptor_length_); 00268 computePoint(point_index, *input_, *normals_, output[point_index].rf, output[point_index].descriptor); 00269 } 00270 } 00271 else 00272 { 00273 size_t block_size = descriptor_length_ / azimuth_bins_; // Size of each L-block 00274 for(size_t point_index = 0; point_index < indices_->size (); point_index++) 00275 { 00276 output[point_index].descriptor.resize (descriptor_length_); 00277 computePoint(point_index, *input_, *normals_, output[point_index].rf, output[point_index].descriptor); 00278 shiftAlongAzimuth(block_size, output[point_index].descriptor); 00279 } 00280 } 00281 } 00282 00283 #define PCL_INSTANTIATE_ShapeContext3DEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::ShapeContext3DEstimation<T,NT,OutT>; 00284 00285 #endif