Point Cloud Library (PCL)  1.9.1-dev
gasd.hpp
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 #ifndef PCL_FEATURES_IMPL_GASD_H_
40 #define PCL_FEATURES_IMPL_GASD_H_
41 
42 #include <pcl/features/gasd.h>
43 #include <pcl/common/transforms.h>
44 #include <pcl/point_types_conversion.h>
45 
46 #include <vector>
47 
48 //////////////////////////////////////////////////////////////////////////////////////////////
49 template <typename PointInT, typename PointOutT> void
51 {
53  {
54  output.width = output.height = 0;
55  output.points.clear ();
56  return;
57  }
58 
59  // Resize the output dataset
60  output.resize (1);
61 
62  // Copy header and is_dense flag from input
63  output.header = surface_->header;
64  output.is_dense = surface_->is_dense;
65 
66  // Perform the actual feature computation
67  computeFeature (output);
68 
70 }
71 
72 //////////////////////////////////////////////////////////////////////////////////////////////
73 template <typename PointInT, typename PointOutT> void
75 {
76  Eigen::Vector4f centroid;
77  Eigen::Matrix3f covariance_matrix;
78 
79  // compute centroid of the object's partial view
80  pcl::compute3DCentroid (*surface_, *indices_, centroid);
81 
82  // compute covariance matrix from points and centroid of the object's partial view
83  pcl::computeCovarianceMatrix (*surface_, *indices_, centroid, covariance_matrix);
84 
85  Eigen::Matrix3f eigenvectors;
86  Eigen::Vector3f eigenvalues;
87 
88  // compute eigenvalues and eigenvectors of the covariance matrix
89  pcl::eigen33 (covariance_matrix, eigenvectors, eigenvalues);
90 
91  // z axis of the reference frame is the eigenvector associated with the minimal eigenvalue
92  Eigen::Vector3f z_axis = eigenvectors.col (0);
93 
94  // if angle between z axis and viewing direction is in the [-90 deg, 90 deg] range, then z axis is negated
95  if (z_axis.dot (view_direction_) > 0)
96  {
97  z_axis = -z_axis;
98  }
99 
100  // x axis of the reference frame is the eigenvector associated with the maximal eigenvalue
101  const Eigen::Vector3f x_axis = eigenvectors.col (2);
102 
103  // y axis is the cross product of z axis and x axis
104  const Eigen::Vector3f y_axis = z_axis.cross (x_axis);
105 
106  const Eigen::Vector3f centroid_xyz = centroid.head<3> ();
107 
108  // compute alignment transform from axes and centroid
109  transform_ << x_axis.transpose (), -x_axis.dot (centroid_xyz),
110  y_axis.transpose (), -y_axis.dot (centroid_xyz),
111  z_axis.transpose (), -z_axis.dot (centroid_xyz),
112  0.0f, 0.0f, 0.0f, 1.0f;
113 }
114 
115 //////////////////////////////////////////////////////////////////////////////////////////////
116 template <typename PointInT, typename PointOutT> void
118  const float max_coord,
119  const size_t half_grid_size,
120  const HistogramInterpolationMethod interp,
121  const float hbin,
122  const float hist_incr,
123  std::vector<Eigen::VectorXf> &hists)
124 {
125  const size_t grid_size = half_grid_size * 2;
126 
127  // compute normalized coordinates with respect to axis-aligned bounding cube centered on the origin
128  const Eigen::Vector3f scaled ( (p[0] / max_coord) * half_grid_size, (p[1] / max_coord) * half_grid_size, (p[2] / max_coord) * half_grid_size);
129 
130  // compute histograms array coords
131  Eigen::Vector4f coords (scaled[0] + half_grid_size, scaled[1] + half_grid_size, scaled[2] + half_grid_size, hbin);
132 
133  // if using histogram interpolation, subtract 0.5 so samples with the central value of the bin have full weight in it
134  if (interp != INTERP_NONE)
135  {
136  coords -= Eigen::Vector4f (0.5f, 0.5f, 0.5f, 0.5f);
137  }
138 
139  // compute histograms bins indices
140  const Eigen::Vector4f bins (std::floor (coords[0]), std::floor (coords[1]), std::floor (coords[2]), std::floor (coords[3]));
141 
142  // compute indices of the bin where the sample falls into
143  const size_t grid_idx = ( (bins[0] + 1) * (grid_size + 2) + bins[1] + 1) * (grid_size + 2) + bins[2] + 1;
144  const size_t h_idx = bins[3] + 1;
145 
146  if (interp == INTERP_NONE)
147  {
148  // no interpolation
149  hists[grid_idx][h_idx] += hist_incr;
150  }
151  else
152  {
153  // if using histogram interpolation, compute trilinear interpolation
154  coords -= Eigen::Vector4f (bins[0], bins[1], bins[2], 0.0f);
155 
156  const float v_x1 = hist_incr * coords[0];
157  const float v_x0 = hist_incr - v_x1;
158 
159  const float v_xy11 = v_x1 * coords[1];
160  const float v_xy10 = v_x1 - v_xy11;
161  const float v_xy01 = v_x0 * coords[1];
162  const float v_xy00 = v_x0 - v_xy01;
163 
164  const float v_xyz111 = v_xy11 * coords[2];
165  const float v_xyz110 = v_xy11 - v_xyz111;
166  const float v_xyz101 = v_xy10 * coords[2];
167  const float v_xyz100 = v_xy10 - v_xyz101;
168  const float v_xyz011 = v_xy01 * coords[2];
169  const float v_xyz010 = v_xy01 - v_xyz011;
170  const float v_xyz001 = v_xy00 * coords[2];
171  const float v_xyz000 = v_xy00 - v_xyz001;
172 
173  if (interp == INTERP_TRILINEAR)
174  {
175  // trilinear interpolation
176  hists[grid_idx][h_idx] += v_xyz000;
177  hists[grid_idx + 1][h_idx] += v_xyz001;
178  hists[grid_idx + (grid_size + 2)][h_idx] += v_xyz010;
179  hists[grid_idx + (grid_size + 3)][h_idx] += v_xyz011;
180  hists[grid_idx + (grid_size + 2) * (grid_size + 2)][h_idx] += v_xyz100;
181  hists[grid_idx + (grid_size + 2) * (grid_size + 2) + 1][h_idx] += v_xyz101;
182  hists[grid_idx + (grid_size + 3) * (grid_size + 2)][h_idx] += v_xyz110;
183  hists[grid_idx + (grid_size + 3) * (grid_size + 2) + 1][h_idx] += v_xyz111;
184  }
185  else
186  {
187  // quadrilinear interpolation
188  coords[3] -= bins[3];
189 
190  const float v_xyzh1111 = v_xyz111 * coords[3];
191  const float v_xyzh1110 = v_xyz111 - v_xyzh1111;
192  const float v_xyzh1101 = v_xyz110 * coords[3];
193  const float v_xyzh1100 = v_xyz110 - v_xyzh1101;
194  const float v_xyzh1011 = v_xyz101 * coords[3];
195  const float v_xyzh1010 = v_xyz101 - v_xyzh1011;
196  const float v_xyzh1001 = v_xyz100 * coords[3];
197  const float v_xyzh1000 = v_xyz100 - v_xyzh1001;
198  const float v_xyzh0111 = v_xyz011 * coords[3];
199  const float v_xyzh0110 = v_xyz011 - v_xyzh0111;
200  const float v_xyzh0101 = v_xyz010 * coords[3];
201  const float v_xyzh0100 = v_xyz010 - v_xyzh0101;
202  const float v_xyzh0011 = v_xyz001 * coords[3];
203  const float v_xyzh0010 = v_xyz001 - v_xyzh0011;
204  const float v_xyzh0001 = v_xyz000 * coords[3];
205  const float v_xyzh0000 = v_xyz000 - v_xyzh0001;
206 
207  hists[grid_idx][h_idx] += v_xyzh0000;
208  hists[grid_idx][h_idx + 1] += v_xyzh0001;
209  hists[grid_idx + 1][h_idx] += v_xyzh0010;
210  hists[grid_idx + 1][h_idx + 1] += v_xyzh0011;
211  hists[grid_idx + (grid_size + 2)][h_idx] += v_xyzh0100;
212  hists[grid_idx + (grid_size + 2)][h_idx + 1] += v_xyzh0101;
213  hists[grid_idx + (grid_size + 3)][h_idx] += v_xyzh0110;
214  hists[grid_idx + (grid_size + 3)][h_idx + 1] += v_xyzh0111;
215  hists[grid_idx + (grid_size + 2) * (grid_size + 2)][h_idx] += v_xyzh1000;
216  hists[grid_idx + (grid_size + 2) * (grid_size + 2)][h_idx + 1] += v_xyzh1001;
217  hists[grid_idx + (grid_size + 2) * (grid_size + 2) + 1][h_idx] += v_xyzh1010;
218  hists[grid_idx + (grid_size + 2) * (grid_size + 2) + 1][h_idx + 1] += v_xyzh1011;
219  hists[grid_idx + (grid_size + 3) * (grid_size + 2)][h_idx] += v_xyzh1100;
220  hists[grid_idx + (grid_size + 3) * (grid_size + 2)][h_idx + 1] += v_xyzh1101;
221  hists[grid_idx + (grid_size + 3) * (grid_size + 2) + 1][h_idx] += v_xyzh1110;
222  hists[grid_idx + (grid_size + 3) * (grid_size + 2) + 1][h_idx + 1] += v_xyzh1111;
223  }
224  }
225 }
226 
227 //////////////////////////////////////////////////////////////////////////////////////////////
228 template <typename PointInT, typename PointOutT> void
230  const size_t hists_size,
231  const std::vector<Eigen::VectorXf> &hists,
232  PointCloudOut &output,
233  size_t &pos)
234 {
235  for (size_t i = 0; i < grid_size; ++i)
236  {
237  for (size_t j = 0; j < grid_size; ++j)
238  {
239  for (size_t k = 0; k < grid_size; ++k)
240  {
241  const size_t idx = ( (i + 1) * (grid_size + 2) + (j + 1)) * (grid_size + 2) + (k + 1);
242 
243  std::copy (hists[idx].data () + 1, hists[idx].data () + hists_size + 1, output.points[0].histogram + pos);
244  pos += hists_size;
245  }
246  }
247  }
248 }
249 
250 //////////////////////////////////////////////////////////////////////////////////////////////
251 template <typename PointInT, typename PointOutT> void
253 {
254  // compute alignment transform using reference frame
255  computeAlignmentTransform ();
256 
257  // align point cloud
258  pcl::transformPointCloud (*surface_, *indices_, shape_samples_, transform_);
259 
260  const size_t shape_grid_size = shape_half_grid_size_ * 2;
261 
262  // each histogram dimension has 2 additional bins, 1 in each boundary, for performing interpolation
263  std::vector<Eigen::VectorXf> shape_hists ((shape_grid_size + 2) * (shape_grid_size + 2) * (shape_grid_size + 2),
264  Eigen::VectorXf::Zero (shape_hists_size_ + 2));
265 
266  Eigen::Vector4f centroid_p = Eigen::Vector4f::Zero ();
267 
268  // compute normalization factor for distances between samples and centroid
269  Eigen::Vector4f far_pt;
270  pcl::getMaxDistance (shape_samples_, centroid_p, far_pt);
271  far_pt[3] = 0;
272  const float distance_normalization_factor = (centroid_p - far_pt).norm ();
273 
274  // compute normalization factor with respect to axis-aligned bounding cube centered on the origin
275  Eigen::Vector4f min_pt, max_pt;
276  pcl::getMinMax3D (shape_samples_, min_pt, max_pt);
277 
278  max_coord_ = std::max (min_pt.head<3> ().cwiseAbs ().maxCoeff (), max_pt.head<3> ().cwiseAbs ().maxCoeff ());
279 
280  // normalize sample contribution with respect to the total number of points in the cloud
281  hist_incr_ = 100.0f / static_cast<float> (shape_samples_.size () - 1);
282 
283  // for each sample
284  for (size_t i = 0; i < shape_samples_.size (); ++i)
285  {
286  // compute shape histogram array coord based on distance between sample and centroid
287  const Eigen::Vector4f p (shape_samples_[i].x, shape_samples_[i].y, shape_samples_[i].z, 0.0f);
288  const float d = p.norm ();
289 
290  const float shape_grid_step = distance_normalization_factor / shape_half_grid_size_;
291 
292  const float dist_hist_start = ((int) (d / shape_grid_step)) * shape_grid_step;
293 
294  const float dist_hist_val = (d - dist_hist_start) / shape_grid_step;
295 
296  const float dbin = dist_hist_val * shape_hists_size_;
297 
298  // add sample to shape histograms, optionally performing interpolation
299  addSampleToHistograms (p, max_coord_, shape_half_grid_size_, shape_interp_, dbin, hist_incr_, shape_hists);
300  }
301 
302  pos_ = 0;
303 
304  // copy shape histograms to output
305  copyShapeHistogramsToOutput (shape_grid_size, shape_hists_size_, shape_hists, output, pos_);
306 
307  // set remaining values of the descriptor to zero (if any)
308  std::fill (output.points[0].histogram + pos_, output.points[0].histogram + output.points[0].descriptorSize (), 0.0f);
309 }
310 
311 //////////////////////////////////////////////////////////////////////////////////////////////
312 template <typename PointInT, typename PointOutT> void
314  const size_t hists_size,
315  std::vector<Eigen::VectorXf> &hists,
316  PointCloudOut &output,
317  size_t &pos)
318 {
319  for (size_t i = 0; i < grid_size; ++i)
320  {
321  for (size_t j = 0; j < grid_size; ++j)
322  {
323  for (size_t k = 0; k < grid_size; ++k)
324  {
325  const size_t idx = ( (i + 1) * (grid_size + 2) + (j + 1)) * (grid_size + 2) + (k + 1);
326 
327  hists[idx][1] += hists[idx][hists_size + 1];
328  hists[idx][hists_size] += hists[idx][0];
329 
330  std::copy (hists[idx].data () + 1, hists[idx].data () + hists_size + 1, output.points[0].histogram + pos);
331  pos += hists_size;
332  }
333  }
334  }
335 }
336 
337 //////////////////////////////////////////////////////////////////////////////////////////////
338 template <typename PointInT, typename PointOutT> void
340 {
341  // call shape feature computation
343 
344  const size_t color_grid_size = color_half_grid_size_ * 2;
345 
346  // each histogram dimension has 2 additional bins, 1 in each boundary, for performing interpolation
347  std::vector<Eigen::VectorXf> color_hists ((color_grid_size + 2) * (color_grid_size + 2) * (color_grid_size + 2),
348  Eigen::VectorXf::Zero (color_hists_size_ + 2));
349 
350  // for each sample
351  for (size_t i = 0; i < shape_samples_.size (); ++i)
352  {
353  // compute shape histogram array coord based on distance between sample and centroid
354  const Eigen::Vector4f p (shape_samples_[i].x, shape_samples_[i].y, shape_samples_[i].z, 0.0f);
355 
356  // compute hue value
357  float hue = 0.f;
358 
359  const unsigned char max = std::max (shape_samples_[i].r, std::max (shape_samples_[i].g, shape_samples_[i].b));
360  const unsigned char min = std::min (shape_samples_[i].r, std::min (shape_samples_[i].g, shape_samples_[i].b));
361 
362  const float diff_inv = 1.f / static_cast <float> (max - min);
363 
364  if (std::isfinite (diff_inv))
365  {
366  if (max == shape_samples_[i].r)
367  {
368  hue = 60.f * (static_cast <float> (shape_samples_[i].g - shape_samples_[i].b) * diff_inv);
369  }
370  else if (max == shape_samples_[i].g)
371  {
372  hue = 60.f * (2.f + static_cast <float> (shape_samples_[i].b - shape_samples_[i].r) * diff_inv);
373  }
374  else
375  {
376  hue = 60.f * (4.f + static_cast <float> (shape_samples_[i].r - shape_samples_[i].g) * diff_inv); // max == b
377  }
378 
379  if (hue < 0.f)
380  {
381  hue += 360.f;
382  }
383  }
384 
385  // compute color histogram array coord based on hue value
386  const float hbin = (hue / 360) * color_hists_size_;
387 
388  // add sample to color histograms, optionally performing interpolation
389  GASDEstimation<PointInT, PointOutT>::addSampleToHistograms (p, max_coord_, color_half_grid_size_, color_interp_, hbin, hist_incr_, color_hists);
390  }
391 
392  // copy color histograms to output
393  copyColorHistogramsToOutput (color_grid_size, color_hists_size_, color_hists, output, pos_);
394 
395  // set remaining values of the descriptor to zero (if any)
396  std::fill (output.points[0].histogram + pos_, output.points[0].histogram + output.points[0].descriptorSize (), 0.0f);
397 }
398 
399 #define PCL_INSTANTIATE_GASDEstimation(InT, OutT) template class PCL_EXPORTS pcl::GASDEstimation<InT, OutT>;
400 #define PCL_INSTANTIATE_GASDColorEstimation(InT, OutT) template class PCL_EXPORTS pcl::GASDColorEstimation<InT, OutT>;
401 
402 #endif // PCL_FEATURES_IMPL_GASD_H_
void computeFeature(PointCloudOut &output) override
Estimate GASD descriptor.
Definition: gasd.hpp:252
trilinear interpolation
Definition: gasd.h:51
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:426
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
Definition: gasd.hpp:50
void resize(std::size_t n)
Resize the cloud.
Definition: point_cloud.h:471
GASDColorEstimation estimates the Globally Aligned Spatial Distribution (GASD) descriptor for a given...
Definition: gasd.h:258
void getMaxDistance(const pcl::PointCloud< PointT > &cloud, const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt)
Get the point at maximum distance from a given point and a given pointcloud.
Definition: common.hpp:144
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:431
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:429
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields=true)
Apply an affine transform defined by an Eigen Transform.
Definition: transforms.hpp:215
unsigned int computeCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > &centroid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute the 3x3 covariance matrix of a given set of points.
void getMinMax3D(const pcl::PointCloud< PointT > &cloud, PointT &min_pt, PointT &max_pt)
Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud...
Definition: common.hpp:242
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:423
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
Definition: eigen.hpp:253
HistogramInterpolationMethod
Different histogram interpolation methods.
Definition: gasd.h:48
no interpolation
Definition: gasd.h:50
GASDEstimation estimates the Globally Aligned Spatial Distribution (GASD) descriptor for a given poin...
Definition: gasd.h:76
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
Definition: point_cloud.h:434
void addSampleToHistograms(const Eigen::Vector4f &p, const float max_coord, const size_t half_grid_size, const HistogramInterpolationMethod interp, const float hbin, const float hist_incr, std::vector< Eigen::VectorXf > &hists)
add a sample to its respective histogram, optionally performing interpolation.
Definition: gasd.hpp:117
Feature represents the base feature class.
Definition: feature.h:105
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
Definition: centroid.hpp:50