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_USC_HPP_ 00042 #define PCL_FEATURES_IMPL_USC_HPP_ 00043 00044 #include <pcl/features/usc.h> 00045 #include <pcl/features/shot_lrf.h> 00046 #include <pcl/common/geometry.h> 00047 #include <pcl/common/angles.h> 00048 #include <pcl/common/utils.h> 00049 00050 ////////////////////////////////////////////////////////////////////////////////////////////// 00051 template <typename PointInT, typename PointOutT, typename PointRFT> bool 00052 pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::initCompute () 00053 { 00054 if (!Feature<PointInT, PointOutT>::initCompute ()) 00055 { 00056 PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); 00057 return (false); 00058 } 00059 00060 // Default LRF estimation alg: SHOTLocalReferenceFrameEstimation 00061 typename SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>::Ptr lrf_estimator(new SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>()); 00062 lrf_estimator->setRadiusSearch (local_radius_); 00063 lrf_estimator->setInputCloud (input_); 00064 lrf_estimator->setIndices (indices_); 00065 if (!fake_surface_) 00066 lrf_estimator->setSearchSurface(surface_); 00067 00068 if (!FeatureWithLocalReferenceFrames<PointInT, PointRFT>::initLocalReferenceFrames (indices_->size (), lrf_estimator)) 00069 { 00070 PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); 00071 return (false); 00072 } 00073 00074 if (search_radius_< min_radius_) 00075 { 00076 PCL_ERROR ("[pcl::%s::initCompute] search_radius_ must be GREATER than min_radius_.\n", getClassName ().c_str ()); 00077 return (false); 00078 } 00079 00080 // Update descriptor length 00081 descriptor_length_ = elevation_bins_ * azimuth_bins_ * radius_bins_; 00082 00083 // Compute radial, elevation and azimuth divisions 00084 float azimuth_interval = 360.0f / static_cast<float> (azimuth_bins_); 00085 float elevation_interval = 180.0f / static_cast<float> (elevation_bins_); 00086 00087 // Reallocate divisions and volume lut 00088 radii_interval_.clear (); 00089 phi_divisions_.clear (); 00090 theta_divisions_.clear (); 00091 volume_lut_.clear (); 00092 00093 // Fills radii interval based on formula (1) in section 2.1 of Frome's paper 00094 radii_interval_.resize (radius_bins_ + 1); 00095 for (size_t j = 0; j < radius_bins_ + 1; j++) 00096 radii_interval_[j] = static_cast<float> (exp (log (min_radius_) + ((static_cast<float> (j) / static_cast<float> (radius_bins_)) * log (search_radius_/min_radius_)))); 00097 00098 // Fill theta didvisions of elevation 00099 theta_divisions_.resize (elevation_bins_+1); 00100 for (size_t k = 0; k < elevation_bins_+1; k++) 00101 theta_divisions_[k] = static_cast<float> (k) * elevation_interval; 00102 00103 // Fill phi didvisions of elevation 00104 phi_divisions_.resize (azimuth_bins_+1); 00105 for (size_t l = 0; l < azimuth_bins_+1; l++) 00106 phi_divisions_[l] = static_cast<float> (l) * azimuth_interval; 00107 00108 // LookUp Table that contains the volume of all the bins 00109 // "phi" term of the volume integral 00110 // "integr_phi" has always the same value so we compute it only one time 00111 float integr_phi = pcl::deg2rad (phi_divisions_[1]) - pcl::deg2rad (phi_divisions_[0]); 00112 // exponential to compute the cube root using pow 00113 float e = 1.0f / 3.0f; 00114 // Resize volume look up table 00115 volume_lut_.resize (radius_bins_ * elevation_bins_ * azimuth_bins_); 00116 // Fill volumes look up table 00117 for (size_t j = 0; j < radius_bins_; j++) 00118 { 00119 // "r" term of the volume integral 00120 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); 00121 00122 for (size_t k = 0; k < elevation_bins_; k++) 00123 { 00124 // "theta" term of the volume integral 00125 float integr_theta = cosf (deg2rad (theta_divisions_[k])) - cosf (deg2rad (theta_divisions_[k+1])); 00126 // Volume 00127 float V = integr_phi * integr_theta * integr_r; 00128 // Compute cube root of the computed volume commented for performance but left 00129 // here for clarity 00130 // float cbrt = pow(V, e); 00131 // cbrt = 1 / cbrt; 00132 00133 for (size_t l = 0; l < azimuth_bins_; l++) 00134 // Store in lut 1/cbrt 00135 //volume_lut_[ (l*elevation_bins_*radius_bins_) + k*radius_bins_ + j ] = cbrt; 00136 volume_lut_[(l*elevation_bins_*radius_bins_) + k*radius_bins_ + j] = 1.0f / powf (V, e); 00137 } 00138 } 00139 return (true); 00140 } 00141 00142 ////////////////////////////////////////////////////////////////////////////////////////////// 00143 template <typename PointInT, typename PointOutT, typename PointRFT> void 00144 pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::computePointDescriptor (size_t index, /*float rf[9],*/ std::vector<float> &desc) 00145 { 00146 pcl::Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap (); 00147 00148 const Eigen::Vector3f x_axis (frames_->points[index].x_axis[0], 00149 frames_->points[index].x_axis[1], 00150 frames_->points[index].x_axis[2]); 00151 //const Eigen::Vector3f& y_axis = frames_->points[index].y_axis.getNormalVector3fMap (); 00152 const Eigen::Vector3f normal (frames_->points[index].z_axis[0], 00153 frames_->points[index].z_axis[1], 00154 frames_->points[index].z_axis[2]); 00155 00156 // Find every point within specified search_radius_ 00157 std::vector<int> nn_indices; 00158 std::vector<float> nn_dists; 00159 const size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists); 00160 // For each point within radius 00161 for (size_t ne = 0; ne < neighb_cnt; ne++) 00162 { 00163 if (pcl::utils::equal(nn_dists[ne], 0.0f)) 00164 continue; 00165 // Get neighbours coordinates 00166 Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap (); 00167 00168 // ----- Compute current neighbour polar coordinates ----- 00169 00170 // Get distance between the neighbour and the origin 00171 float r = sqrtf (nn_dists[ne]); 00172 00173 // Project point into the tangent plane 00174 Eigen::Vector3f proj; 00175 pcl::geometry::project (neighbour, origin, normal, proj); 00176 proj -= origin; 00177 00178 // Normalize to compute the dot product 00179 proj.normalize (); 00180 00181 // Compute the angle between the projection and the x axis in the interval [0,360] 00182 Eigen::Vector3f cross = x_axis.cross (proj); 00183 float phi = rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj))); 00184 phi = cross.dot (normal) < 0.f ? (360.0f - phi) : phi; 00185 /// Compute the angle between the neighbour and the z axis (normal) in the interval [0, 180] 00186 Eigen::Vector3f no = neighbour - origin; 00187 no.normalize (); 00188 float theta = normal.dot (no); 00189 theta = pcl::rad2deg (acosf (std::min (1.0f, std::max (-1.0f, theta)))); 00190 00191 /// Bin (j, k, l) 00192 size_t j = 0; 00193 size_t k = 0; 00194 size_t l = 0; 00195 00196 /// Compute the Bin(j, k, l) coordinates of current neighbour 00197 for (size_t rad = 1; rad < radius_bins_ + 1; rad++) 00198 { 00199 if (r <= radii_interval_[rad]) 00200 { 00201 j = rad - 1; 00202 break; 00203 } 00204 } 00205 00206 for (size_t ang = 1; ang < elevation_bins_ + 1; ang++) 00207 { 00208 if (theta <= theta_divisions_[ang]) 00209 { 00210 k = ang - 1; 00211 break; 00212 } 00213 } 00214 00215 for (size_t ang = 1; ang < azimuth_bins_ + 1; ang++) 00216 { 00217 if (phi <= phi_divisions_[ang]) 00218 { 00219 l = ang - 1; 00220 break; 00221 } 00222 } 00223 00224 /// Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour 00225 std::vector<int> neighbour_indices; 00226 std::vector<float> neighbour_didtances; 00227 float point_density = static_cast<float> (searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_didtances)); 00228 /// point_density is always bigger than 0 because FindPointsWithinRadius returns at least the point itself 00229 float w = (1.0f / point_density) * volume_lut_[(l*elevation_bins_*radius_bins_) + 00230 (k*radius_bins_) + 00231 j]; 00232 00233 assert (w >= 0.0); 00234 if (w == std::numeric_limits<float>::infinity ()) 00235 PCL_ERROR ("Shape Context Error INF!\n"); 00236 if (w != w) 00237 PCL_ERROR ("Shape Context Error IND!\n"); 00238 /// Accumulate w into correspondant Bin(j,k,l) 00239 desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w; 00240 00241 assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0); 00242 } // end for each neighbour 00243 } 00244 00245 ////////////////////////////////////////////////////////////////////////////////////////////// 00246 template <typename PointInT, typename PointOutT, typename PointRFT> void 00247 pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::computeFeature (PointCloudOut &output) 00248 { 00249 assert (descriptor_length_ == 1980); 00250 00251 output.is_dense = true; 00252 00253 for (size_t point_index = 0; point_index < indices_->size (); ++point_index) 00254 { 00255 //output[point_index].descriptor.resize (descriptor_length_); 00256 00257 // If the point is not finite, set the descriptor to NaN and continue 00258 const PointRFT& current_frame = (*frames_)[point_index]; 00259 if (!isFinite ((*input_)[(*indices_)[point_index]]) || 00260 !pcl_isfinite (current_frame.x_axis[0]) || 00261 !pcl_isfinite (current_frame.y_axis[0]) || 00262 !pcl_isfinite (current_frame.z_axis[0]) ) 00263 { 00264 for (size_t i = 0; i < descriptor_length_; ++i) 00265 output[point_index].descriptor[i] = std::numeric_limits<float>::quiet_NaN (); 00266 00267 memset (output[point_index].rf, 0, sizeof (output[point_index].rf[0]) * 9); 00268 output.is_dense = false; 00269 continue; 00270 } 00271 00272 for (int d = 0; d < 3; ++d) 00273 { 00274 output.points[point_index].rf[0 + d] = current_frame.x_axis[d]; 00275 output.points[point_index].rf[3 + d] = current_frame.y_axis[d]; 00276 output.points[point_index].rf[6 + d] = current_frame.z_axis[d]; 00277 } 00278 00279 std::vector<float> descriptor (descriptor_length_); 00280 computePointDescriptor (point_index, descriptor); 00281 for (size_t j = 0; j < descriptor_length_; ++j) 00282 output [point_index].descriptor[j] = descriptor[j]; 00283 } 00284 } 00285 00286 #define PCL_INSTANTIATE_UniqueShapeContext(T,OutT,RFT) template class PCL_EXPORTS pcl::UniqueShapeContext<T,OutT,RFT>; 00287 00288 #endif