1 /* 2 * Software License Agreement (BSD License) 3 * 4 * Point Cloud Library (PCL) - www.pointclouds.org 5 * Copyright (c) 2016-, Open Perception, Inc. 6 * Copyright (c) 2016, Voxar Labs, CIn-UFPE / DEINFO-UFRPE 7 * 8 * All rights reserved. 9 * 10 * Redistribution and use in source and binary forms, with or without 11 * modification, are permitted provided that the following conditions 12 * are met: 13 * 14 * * Redistributions of source code must retain the above copyright 15 * notice, this list of conditions and the following disclaimer. 16 * * Redistributions in binary form must reproduce the above 17 * copyright notice, this list of conditions and the following 18 * disclaimer in the documentation and/or other materials provided 19 * with the distribution. 20 * * Neither the name of the copyright holder(s) nor the names of its 21 * contributors may be used to endorse or promote products derived 22 * from this software without specific prior written permission. 23 * 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 * POSSIBILITY OF SUCH DAMAGE. 36 * 37 */ 38 39 #pragma once 40 41 #include <pcl/features/feature.h> 42 43 namespace pcl 44 { 45 /// Different histogram interpolation methods 46 enum HistogramInterpolationMethod 47 { 48 INTERP_NONE, ///< no interpolation 49 INTERP_TRILINEAR, ///< trilinear interpolation 50 INTERP_QUADRILINEAR ///< quadrilinear interpolation 51 }; 52 53 /** \brief GASDEstimation estimates the Globally Aligned Spatial Distribution (GASD) descriptor for a given 54 * point cloud dataset given XYZ data. 55 * 56 * The suggested PointOutT is pcl::GASDSignature512. 57 * 58 * \note If you use this code in any academic work, please cite: 59 * 60 * - J. Lima, V. Teichrieb. 61 * An Efficient Global Point Cloud Descriptor for Object Recognition and Pose Estimation. 62 * In Proceedings of the 29th SIBGRAPI - Conference on Graphics, Patterns and Images, 63 * Sao Jose dos Campos, Brazil, October 4-7 2016. 64 * 65 * \author Joao Paulo Lima 66 * 67 * Voxar Labs, Centro de Informatica, Universidade Federal de Pernambuco, Brazil 68 * 69 * Departamento de Estatistica e Informatica, Universidade Federal Rural de Pernambuco, Brazil 70 * 71 * \ingroup features 72 */ 73 template <typename PointInT, typename PointOutT = GASDSignature512> 74 class GASDEstimation : public Feature<PointInT, PointOutT> 75 { 76 public: 77 using typename Feature<PointInT, PointOutT>::PointCloudIn; 78 using typename Feature<PointInT, PointOutT>::PointCloudOut; 79 using Ptr = shared_ptr<GASDEstimation<PointInT, PointOutT> >; 80 using ConstPtr = shared_ptr<const GASDEstimation<PointInT, PointOutT> >; 81 82 /** \brief Constructor. 83 * \param[in] view_direction view direction 84 * \param[in] shape_half_grid_size shape half grid size 85 * \param[in] shape_hists_size shape histograms size 86 * \param[in] shape_interp shape histograms interpolation method 87 */ 88 GASDEstimation (const Eigen::Vector3f &view_direction = Eigen::Vector3f (0.0f, 0.0f, 1.0f), 89 const std::size_t shape_half_grid_size = 4, 90 const std::size_t shape_hists_size = 1, 91 const HistogramInterpolationMethod shape_interp = INTERP_TRILINEAR) : view_direction_(view_direction)92 view_direction_ (view_direction), 93 shape_half_grid_size_ (shape_half_grid_size), 94 shape_hists_size_ (shape_hists_size), 95 shape_interp_ (shape_interp) 96 { 97 search_radius_ = 0; 98 k_ = 1; 99 feature_name_ = "GASDEstimation"; 100 } 101 102 /** \brief Set the view direction. 103 * \param[in] dir view direction 104 */ 105 inline void setViewDirection(const Eigen::Vector3f & dir)106 setViewDirection (const Eigen::Vector3f &dir) 107 { 108 view_direction_ = dir; 109 } 110 111 /** \brief Set the shape half grid size. 112 * \param[in] shgs shape half grid size 113 */ 114 inline void setShapeHalfGridSize(const std::size_t shgs)115 setShapeHalfGridSize (const std::size_t shgs) 116 { 117 shape_half_grid_size_ = shgs; 118 } 119 120 /** \brief Set the shape histograms size. If size is 1, then each histogram bin will store the number 121 * of points that belong to its correspondent cell in the 3D regular grid. If size > 1, then for each cell 122 * it will be computed a histogram of normalized distances between each sample and the cloud centroid 123 * \param[in] shs shape histograms size 124 */ 125 inline void setShapeHistsSize(const std::size_t shs)126 setShapeHistsSize (const std::size_t shs) 127 { 128 shape_hists_size_ = shs; 129 } 130 131 /** \brief Set the shape histograms interpolation method. 132 * \param[in] interp shape histograms interpolation method 133 */ 134 inline void setShapeHistsInterpMethod(const HistogramInterpolationMethod interp)135 setShapeHistsInterpMethod (const HistogramInterpolationMethod interp) 136 { 137 shape_interp_ = interp; 138 } 139 140 /** 141 * \brief Returns the transformation aligning the point cloud to the canonical coordinate system 142 */ 143 const Eigen::Matrix4f& getTransform()144 getTransform () const 145 { 146 return transform_; 147 } 148 149 /** \brief Overloaded computed method from pcl::Feature. 150 * \param[out] output the resultant point cloud model dataset containing the estimated feature 151 */ 152 void 153 compute (PointCloudOut &output); 154 155 protected: 156 using Feature<PointInT, PointOutT>::feature_name_; 157 using Feature<PointInT, PointOutT>::getClassName; 158 using Feature<PointInT, PointOutT>::indices_; 159 using Feature<PointInT, PointOutT>::k_; 160 using Feature<PointInT, PointOutT>::search_radius_; 161 using Feature<PointInT, PointOutT>::surface_; 162 163 /** \brief Point cloud aligned to the canonical coordinate system. */ 164 PointCloudIn shape_samples_; 165 166 /** \brief Normalization factor with respect to axis-aligned bounding cube centered on the origin. */ 167 float max_coord_; 168 169 /** \brief Normalized sample contribution with respect to the total number of points in the cloud. */ 170 float hist_incr_; 171 172 /** \brief Current position of output descriptor point cloud. */ 173 std::size_t pos_; 174 175 /** \brief add a sample to its respective histogram, optionally performing interpolation. 176 * \param[in] p histogram sample 177 * \param[in] max_coord normalization factor with respect to axis-aligned bounding cube centered on the origin 178 * \param[in] half_grid_size half size of the regular grid used to compute the descriptor 179 * \param[in] interp interpolation method to be used while computing the descriptor 180 * \param[in] hbin histogram bin 181 * \param[in] hist_incr normalization factor of sample contribution 182 * \param[in,out] hists updated histograms 183 */ 184 void 185 addSampleToHistograms (const Eigen::Vector4f &p, 186 const float max_coord, 187 const std::size_t half_grid_size, 188 const HistogramInterpolationMethod interp, 189 const float hbin, 190 const float hist_incr, 191 std::vector<Eigen::VectorXf> &hists); 192 193 /** \brief Estimate GASD descriptor 194 * 195 * \param[out] output the resultant point cloud model dataset containing the GASD feature 196 */ 197 void 198 computeFeature (PointCloudOut &output) override; 199 200 private: 201 /** \brief Transform that aligns the point cloud to the canonical coordinate system. */ 202 Eigen::Matrix4f transform_; 203 204 /** \brief Viewing direction, default value is (0, 0, 1). */ 205 Eigen::Vector3f view_direction_; 206 207 /** \brief Half size of the regular grid used to compute the shape descriptor. */ 208 std::size_t shape_half_grid_size_; 209 210 /** \brief Size of the histograms of normalized distances between each sample and the cloud centroid. */ 211 std::size_t shape_hists_size_; 212 213 /** \brief Interpolation method to be used while computing the shape descriptor. */ 214 HistogramInterpolationMethod shape_interp_; 215 216 /** \brief Estimates a reference frame for the point cloud and uses it to compute a transform that aligns the point cloud to the canonical coordinate system. */ 217 void 218 computeAlignmentTransform (); 219 220 /** \brief copy computed shape histograms to output descriptor point cloud 221 * \param[in] grid_size size of the regular grid used to compute the descriptor 222 * \param[in] hists_size size of the shape histograms 223 * \param[in] hists shape histograms 224 * \param[out] output output descriptor point cloud 225 * \param[in,out] pos current position of output descriptor point cloud 226 */ 227 void 228 copyShapeHistogramsToOutput (const std::size_t grid_size, 229 const std::size_t hists_size, 230 const std::vector<Eigen::VectorXf> &hists, 231 PointCloudOut &output, 232 std::size_t &pos); 233 }; 234 235 /** \brief GASDColorEstimation estimates the Globally Aligned Spatial Distribution (GASD) descriptor for a given 236 * point cloud dataset given XYZ and RGB data. 237 * 238 * The suggested PointOutT is pcl::GASDSignature984. 239 * 240 * \note If you use this code in any academic work, please cite: 241 * 242 * - J. Lima, V. Teichrieb. 243 * An Efficient Global Point Cloud Descriptor for Object Recognition and Pose Estimation. 244 * In Proceedings of the 29th SIBGRAPI - Conference on Graphics, Patterns and Images, 245 * Sao Jose dos Campos, Brazil, October 4-7 2016. 246 * 247 * \author Joao Paulo Lima 248 * 249 * Voxar Labs, Centro de Informatica, Universidade Federal de Pernambuco, Brazil 250 * 251 * Departamento de Estatistica e Informatica, Universidade Federal Rural de Pernambuco, Brazil 252 * 253 * \ingroup features 254 */ 255 template <typename PointInT, typename PointOutT = GASDSignature984> 256 class GASDColorEstimation : public GASDEstimation<PointInT, PointOutT> 257 { 258 public: 259 using typename Feature<PointInT, PointOutT>::PointCloudOut; 260 using Ptr = shared_ptr<GASDColorEstimation<PointInT, PointOutT> >; 261 using ConstPtr = shared_ptr<const GASDColorEstimation<PointInT, PointOutT> >; 262 263 /** \brief Constructor. 264 * \param[in] view_direction view direction 265 * \param[in] shape_half_grid_size shape half grid size 266 * \param[in] shape_hists_size shape histograms size 267 * \param[in] color_half_grid_size color half grid size 268 * \param[in] color_hists_size color histograms size 269 * \param[in] shape_interp shape histograms interpolation method 270 * \param[in] color_interp color histograms interpolation method 271 */ 272 GASDColorEstimation (const Eigen::Vector3f &view_direction = Eigen::Vector3f (0.0f, 0.0f, 1.0f), 273 const std::size_t shape_half_grid_size = 3, 274 const std::size_t shape_hists_size = 1, 275 const std::size_t color_half_grid_size = 2, 276 const std::size_t color_hists_size = 12, 277 const HistogramInterpolationMethod shape_interp = INTERP_NONE, 278 const HistogramInterpolationMethod color_interp = INTERP_NONE) : 279 GASDEstimation<PointInT, PointOutT> (view_direction, shape_half_grid_size, shape_hists_size, shape_interp), 280 color_half_grid_size_ (color_half_grid_size), 281 color_hists_size_ (color_hists_size), 282 color_interp_ (color_interp) 283 { 284 feature_name_ = "GASDColorEstimation"; 285 } 286 287 /** \brief Set the color half grid size. 288 * \param[in] chgs color half grid size 289 */ 290 inline void setColorHalfGridSize(const std::size_t chgs)291 setColorHalfGridSize (const std::size_t chgs) 292 { 293 color_half_grid_size_ = chgs; 294 } 295 296 /** \brief Set the color histograms size (number of bins in the hue histogram for each cell of the 3D regular grid). 297 * \param[in] chs color histograms size 298 */ 299 inline void setColorHistsSize(const std::size_t chs)300 setColorHistsSize (const std::size_t chs) 301 { 302 color_hists_size_ = chs; 303 } 304 305 /** \brief Set the color histograms interpolation method. 306 * \param[in] interp color histograms interpolation method 307 */ 308 inline void setColorHistsInterpMethod(const HistogramInterpolationMethod interp)309 setColorHistsInterpMethod (const HistogramInterpolationMethod interp) 310 { 311 color_interp_ = interp; 312 } 313 314 protected: 315 using Feature<PointInT, PointOutT>::feature_name_; 316 using Feature<PointInT, PointOutT>::getClassName; 317 using Feature<PointInT, PointOutT>::indices_; 318 using Feature<PointInT, PointOutT>::k_; 319 using Feature<PointInT, PointOutT>::search_radius_; 320 using Feature<PointInT, PointOutT>::surface_; 321 using GASDEstimation<PointInT, PointOutT>::shape_samples_; 322 using GASDEstimation<PointInT, PointOutT>::max_coord_; 323 using GASDEstimation<PointInT, PointOutT>::hist_incr_; 324 using GASDEstimation<PointInT, PointOutT>::pos_; 325 326 private: 327 /** \brief Half size of the regular grid used to compute the color descriptor. */ 328 std::size_t color_half_grid_size_; 329 330 /** \brief Size of the hue histograms. */ 331 std::size_t color_hists_size_; 332 333 /** \brief Interpolation method to be used while computing the color descriptor. */ 334 HistogramInterpolationMethod color_interp_; 335 336 /** \brief copy computed color histograms to output descriptor point cloud 337 * \param[in] grid_size size of the regular grid used to compute the descriptor 338 * \param[in] hists_size size of the color histograms 339 * \param[in,out] hists color histograms, which are finalized, since they are circular 340 * \param[out] output output descriptor point cloud 341 * \param[in,out] pos current position of output descriptor point cloud 342 */ 343 void 344 copyColorHistogramsToOutput (const std::size_t grid_size, 345 const std::size_t hists_size, 346 std::vector<Eigen::VectorXf> &hists, 347 PointCloudOut &output, 348 std::size_t &pos); 349 350 /** \brief Estimate GASD color descriptor 351 * 352 * \param[out] output the resultant point cloud model dataset containing the GASD color feature 353 */ 354 void 355 computeFeature (PointCloudOut &output) override; 356 }; 357 } // namespace pcl 358 359 #ifdef PCL_NO_PRECOMPILE 360 #include <pcl/features/impl/gasd.hpp> 361 #endif 362