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