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) 2009, 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: gfpfh.hpp 2218 2011-08-25 20:27:15Z rusu $ 00038 * 00039 */ 00040 00041 #ifndef PCL_FEATURES_IMPL_GFPFH_H_ 00042 #define PCL_FEATURES_IMPL_GFPFH_H_ 00043 00044 #include <pcl/features/gfpfh.h> 00045 #include <pcl/octree/octree.h> 00046 #include <pcl/octree/octree_search.h> 00047 #include <pcl/common/eigen.h> 00048 00049 #include <algorithm> 00050 #include <fstream> 00051 00052 ////////////////////////////////////////////////////////////////////////////////////////////// 00053 template<typename PointInT, typename PointNT, typename PointOutT> void 00054 pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::compute (PointCloudOut &output) 00055 { 00056 if (!Feature<PointInT, PointOutT>::initCompute ()) 00057 { 00058 output.width = output.height = 0; 00059 output.points.clear (); 00060 return; 00061 } 00062 // Copy the header 00063 output.header = input_->header; 00064 00065 // Resize the output dataset 00066 // Important! We should only allocate precisely how many elements we will need, otherwise 00067 // we risk at pre-allocating too much memory which could lead to bad_alloc 00068 // (see http://dev.pointclouds.org/issues/657) 00069 output.width = output.height = 1; 00070 output.is_dense = input_->is_dense; 00071 output.points.resize (1); 00072 00073 // Perform the actual feature computation 00074 computeFeature (output); 00075 00076 Feature<PointInT, PointOutT>::deinitCompute (); 00077 } 00078 00079 ////////////////////////////////////////////////////////////////////////////////////////////// 00080 template <typename PointInT, typename PointNT, typename PointOutT> void 00081 pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output) 00082 { 00083 pcl::octree::OctreePointCloudSearch<PointInT> octree (octree_leaf_size_); 00084 octree.setInputCloud (input_); 00085 octree.addPointsFromInputCloud (); 00086 00087 typename pcl::PointCloud<PointInT>::VectorType occupied_cells; 00088 octree.getOccupiedVoxelCenters (occupied_cells); 00089 00090 // Determine the voxels crosses along the line segments 00091 // formed by every pair of occupied cells. 00092 std::vector< std::vector<int> > line_histograms; 00093 for (size_t i = 0; i < occupied_cells.size (); ++i) 00094 { 00095 Eigen::Vector3f origin = occupied_cells[i].getVector3fMap (); 00096 00097 for (size_t j = i+1; j < occupied_cells.size (); ++j) 00098 { 00099 typename pcl::PointCloud<PointInT>::VectorType intersected_cells; 00100 Eigen::Vector3f end = occupied_cells[j].getVector3fMap (); 00101 octree.getApproxIntersectedVoxelCentersBySegment (origin, end, intersected_cells, 0.5f); 00102 00103 // Intersected cells are ordered from closest to furthest w.r.t. the origin. 00104 std::vector<int> histogram; 00105 for (size_t k = 0; k < intersected_cells.size (); ++k) 00106 { 00107 std::vector<int> indices; 00108 octree.voxelSearch (intersected_cells[k], indices); 00109 int label = emptyLabel (); 00110 if (indices.size () != 0) 00111 { 00112 label = getDominantLabel (indices); 00113 } 00114 histogram.push_back (label); 00115 } 00116 00117 line_histograms.push_back(histogram); 00118 } 00119 } 00120 00121 std::vector< std::vector<int> > transition_histograms; 00122 computeTransitionHistograms (line_histograms, transition_histograms); 00123 00124 std::vector<float> distances; 00125 computeDistancesToMean (transition_histograms, distances); 00126 00127 std::vector<float> gfpfh_histogram; 00128 computeDistanceHistogram (distances, gfpfh_histogram); 00129 00130 output.clear (); 00131 output.width = 1; 00132 output.height = 1; 00133 output.points.resize (1); 00134 std::copy (gfpfh_histogram.begin (), gfpfh_histogram.end (), output.points[0].histogram); 00135 } 00136 00137 ////////////////////////////////////////////////////////////////////////////////////////////// 00138 template <typename PointInT, typename PointNT, typename PointOutT> void 00139 pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeTransitionHistograms (const std::vector< std::vector<int> >& label_histograms, 00140 std::vector< std::vector<int> >& transition_histograms) 00141 { 00142 transition_histograms.resize (label_histograms.size ()); 00143 00144 for (size_t i = 0; i < label_histograms.size (); ++i) 00145 { 00146 transition_histograms[i].resize ((getNumberOfClasses () + 2) * (getNumberOfClasses () + 1) / 2, 0); 00147 00148 std::vector< std::vector <int> > transitions (getNumberOfClasses () + 1); 00149 for (size_t k = 0; k < transitions.size (); ++k) 00150 { 00151 transitions[k].resize (getNumberOfClasses () + 1, 0); 00152 } 00153 00154 for (size_t k = 1; k < label_histograms[i].size (); ++k) 00155 { 00156 uint32_t first_class = label_histograms[i][k-1]; 00157 uint32_t second_class = label_histograms[i][k]; 00158 // Order has no influence. 00159 if (second_class < first_class) 00160 std::swap (first_class, second_class); 00161 00162 transitions[first_class][second_class] += 1; 00163 } 00164 00165 // Build a one-dimension histogram out of it. 00166 int flat_index = 0; 00167 for (int m = 0; m < static_cast<int> (transitions.size ()); ++m) 00168 for (int n = m; n < static_cast<int> (transitions[m].size ()); ++n) 00169 { 00170 transition_histograms[i][flat_index] = transitions[m][n]; 00171 ++flat_index; 00172 } 00173 00174 assert (flat_index == static_cast<int> (transition_histograms[i].size ())); 00175 } 00176 } 00177 00178 ////////////////////////////////////////////////////////////////////////////////////////////// 00179 template <typename PointInT, typename PointNT, typename PointOutT> void 00180 pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeDistancesToMean (const std::vector< std::vector<int> >& transition_histograms, 00181 std::vector<float>& distances) 00182 { 00183 distances.resize (transition_histograms.size ()); 00184 00185 std::vector<float> mean_histogram; 00186 computeMeanHistogram (transition_histograms, mean_histogram); 00187 00188 for (size_t i = 0; i < transition_histograms.size (); ++i) 00189 { 00190 float d = computeHIKDistance (transition_histograms[i], mean_histogram); 00191 distances[i] = d; 00192 } 00193 } 00194 00195 ////////////////////////////////////////////////////////////////////////////////////////////// 00196 template <typename PointInT, typename PointNT, typename PointOutT> void 00197 pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeDistanceHistogram (const std::vector<float>& distances, 00198 std::vector<float>& histogram) 00199 { 00200 std::vector<float>::const_iterator min_it = std::min_element (distances.begin (), distances.end ()); 00201 assert (min_it != distances.end ()); 00202 const float min_value = *min_it; 00203 00204 std::vector<float>::const_iterator max_it = std::max_element (distances.begin (), distances.end ()); 00205 assert (max_it != distances.end()); 00206 const float max_value = *max_it; 00207 00208 histogram.resize (descriptorSize (), 0); 00209 00210 const float range = max_value - min_value; 00211 const int max_bin = descriptorSize () - 1; 00212 for (size_t i = 0; i < distances.size (); ++i) 00213 { 00214 const float raw_bin = static_cast<float> (descriptorSize ()) * (distances[i] - min_value) / range; 00215 int bin = std::min (max_bin, static_cast<int> (floor (raw_bin))); 00216 histogram[bin] += 1; 00217 } 00218 } 00219 00220 ////////////////////////////////////////////////////////////////////////////////////////////// 00221 template <typename PointInT, typename PointNT, typename PointOutT> void 00222 pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeMeanHistogram (const std::vector< std::vector<int> >& histograms, 00223 std::vector<float>& mean_histogram) 00224 { 00225 assert (histograms.size () > 0); 00226 00227 mean_histogram.resize (histograms[0].size (), 0); 00228 for (size_t i = 0; i < histograms.size (); ++i) 00229 for (size_t j = 0; j < histograms[i].size (); ++j) 00230 mean_histogram[j] += static_cast<float> (histograms[i][j]); 00231 00232 for (size_t i = 0; i < mean_histogram.size (); ++i) 00233 mean_histogram[i] /= static_cast<float> (histograms.size ()); 00234 } 00235 00236 ////////////////////////////////////////////////////////////////////////////////////////////// 00237 template <typename PointInT, typename PointNT, typename PointOutT> float 00238 pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeHIKDistance (const std::vector<int>& histogram, 00239 const std::vector<float>& mean_histogram) 00240 { 00241 assert (histogram.size () == mean_histogram.size ()); 00242 00243 float norm = 0.f; 00244 for (size_t i = 0; i < histogram.size (); ++i) 00245 norm += std::min (static_cast<float> (histogram[i]), mean_histogram[i]); 00246 00247 norm /= static_cast<float> (histogram.size ()); 00248 return (norm); 00249 } 00250 00251 ////////////////////////////////////////////////////////////////////////////////////////////// 00252 template <typename PointInT, typename PointNT, typename PointOutT> boost::uint32_t 00253 pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::getDominantLabel (const std::vector<int>& indices) 00254 { 00255 std::vector<uint32_t> counts (getNumberOfClasses () + 1, 0); 00256 for (size_t i = 0; i < indices.size (); ++i) 00257 { 00258 uint32_t label = labels_->points[indices[i]].label; 00259 counts[label] += 1; 00260 } 00261 00262 std::vector<uint32_t>::const_iterator max_it; 00263 max_it = std::max_element (counts.begin (), counts.end ()); 00264 if (max_it == counts.end ()) 00265 return (emptyLabel ()); 00266 00267 return (static_cast<uint32_t> (max_it - counts.begin ())); 00268 } 00269 00270 #define PCL_INSTANTIATE_GFPFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::GFPFHEstimation<T,NT,OutT>; 00271 00272 #endif // PCL_FEATURES_IMPL_GFPFH_H_