Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
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/common.h> // for getMinMax3D
44#include <pcl/common/transforms.h>
45
46#include <vector>
47
48//////////////////////////////////////////////////////////////////////////////////////////////
49template <typename PointInT, typename PointOutT> void
51{
53 {
54 output.width = output.height = 0;
55 output.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//////////////////////////////////////////////////////////////////////////////////////////////
73template <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, then compute covariance matrix from points and centroid of the object's partial view
80 if (pcl::compute3DCentroid (*surface_, *indices_, centroid) == 0 ||
81 pcl::computeCovarianceMatrix (*surface_, *indices_, centroid, covariance_matrix) == 0) {
82 PCL_ERROR("[pcl::GASDEstimation::computeAlignmentTransform] Surface cloud or indices are empty!\n");
83 return;
84 }
85
86 Eigen::Matrix3f eigenvectors;
87 Eigen::Vector3f eigenvalues;
88
89 // compute eigenvalues and eigenvectors of the covariance matrix
90 pcl::eigen33 (covariance_matrix, eigenvectors, eigenvalues);
91
92 // z axis of the reference frame is the eigenvector associated with the minimal eigenvalue
93 Eigen::Vector3f z_axis = eigenvectors.col (0);
94
95 // if angle between z axis and viewing direction is in the [-90 deg, 90 deg] range, then z axis is negated
96 if (z_axis.dot (view_direction_) > 0)
97 {
98 z_axis = -z_axis;
99 }
100
101 // x axis of the reference frame is the eigenvector associated with the maximal eigenvalue
102 const Eigen::Vector3f x_axis = eigenvectors.col (2);
103
104 // y axis is the cross product of z axis and x axis
105 const Eigen::Vector3f y_axis = z_axis.cross (x_axis);
106
107 const Eigen::Vector3f centroid_xyz = centroid.head<3> ();
108
109 // compute alignment transform from axes and centroid
110 transform_ << x_axis.transpose (), -x_axis.dot (centroid_xyz),
111 y_axis.transpose (), -y_axis.dot (centroid_xyz),
112 z_axis.transpose (), -z_axis.dot (centroid_xyz),
113 0.0f, 0.0f, 0.0f, 1.0f;
114}
115
116//////////////////////////////////////////////////////////////////////////////////////////////
117template <typename PointInT, typename PointOutT> void
119 const float max_coord,
120 const std::size_t half_grid_size,
121 const HistogramInterpolationMethod interp,
122 const float hbin,
123 const float hist_incr,
124 std::vector<Eigen::VectorXf> &hists)
125{
126 const std::size_t grid_size = half_grid_size * 2;
127
128 // compute normalized coordinates with respect to axis-aligned bounding cube centered on the origin
129 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);
130
131 // compute histograms array coords
132 Eigen::Vector4f coords (scaled[0] + half_grid_size, scaled[1] + half_grid_size, scaled[2] + half_grid_size, hbin);
133
134 // if using histogram interpolation, subtract 0.5 so samples with the central value of the bin have full weight in it
135 if (interp != INTERP_NONE)
136 {
137 coords -= Eigen::Vector4f (0.5f, 0.5f, 0.5f, 0.5f);
138 }
139
140 // compute histograms bins indices
141 const Eigen::Vector4f bins (std::floor (coords[0]), std::floor (coords[1]), std::floor (coords[2]), std::floor (coords[3]));
142
143 // compute indices of the bin where the sample falls into
144 const std::size_t grid_idx = ( (bins[0] + 1) * (grid_size + 2) + bins[1] + 1) * (grid_size + 2) + bins[2] + 1;
145 const std::size_t h_idx = bins[3] + 1;
146
147 if (interp == INTERP_NONE)
148 {
149 // no interpolation
150 hists[grid_idx][h_idx] += hist_incr;
151 }
152 else
154 // if using histogram interpolation, compute trilinear interpolation
155 coords -= Eigen::Vector4f (bins[0], bins[1], bins[2], 0.0f);
156
157 const float v_x1 = hist_incr * coords[0];
158 const float v_x0 = hist_incr - v_x1;
159
160 const float v_xy11 = v_x1 * coords[1];
161 const float v_xy10 = v_x1 - v_xy11;
162 const float v_xy01 = v_x0 * coords[1];
163 const float v_xy00 = v_x0 - v_xy01;
164
165 const float v_xyz111 = v_xy11 * coords[2];
166 const float v_xyz110 = v_xy11 - v_xyz111;
167 const float v_xyz101 = v_xy10 * coords[2];
168 const float v_xyz100 = v_xy10 - v_xyz101;
169 const float v_xyz011 = v_xy01 * coords[2];
170 const float v_xyz010 = v_xy01 - v_xyz011;
171 const float v_xyz001 = v_xy00 * coords[2];
172 const float v_xyz000 = v_xy00 - v_xyz001;
173
174 if (interp == INTERP_TRILINEAR)
175 {
176 // trilinear interpolation
177 hists[grid_idx][h_idx] += v_xyz000;
178 hists[grid_idx + 1][h_idx] += v_xyz001;
179 hists[grid_idx + (grid_size + 2)][h_idx] += v_xyz010;
180 hists[grid_idx + (grid_size + 3)][h_idx] += v_xyz011;
181 hists[grid_idx + (grid_size + 2) * (grid_size + 2)][h_idx] += v_xyz100;
182 hists[grid_idx + (grid_size + 2) * (grid_size + 2) + 1][h_idx] += v_xyz101;
183 hists[grid_idx + (grid_size + 3) * (grid_size + 2)][h_idx] += v_xyz110;
184 hists[grid_idx + (grid_size + 3) * (grid_size + 2) + 1][h_idx] += v_xyz111;
186 else
187 {
188 // quadrilinear interpolation
189 coords[3] -= bins[3];
190
191 const float v_xyzh1111 = v_xyz111 * coords[3];
192 const float v_xyzh1110 = v_xyz111 - v_xyzh1111;
193 const float v_xyzh1101 = v_xyz110 * coords[3];
194 const float v_xyzh1100 = v_xyz110 - v_xyzh1101;
195 const float v_xyzh1011 = v_xyz101 * coords[3];
196 const float v_xyzh1010 = v_xyz101 - v_xyzh1011;
197 const float v_xyzh1001 = v_xyz100 * coords[3];
198 const float v_xyzh1000 = v_xyz100 - v_xyzh1001;
199 const float v_xyzh0111 = v_xyz011 * coords[3];
200 const float v_xyzh0110 = v_xyz011 - v_xyzh0111;
201 const float v_xyzh0101 = v_xyz010 * coords[3];
202 const float v_xyzh0100 = v_xyz010 - v_xyzh0101;
203 const float v_xyzh0011 = v_xyz001 * coords[3];
204 const float v_xyzh0010 = v_xyz001 - v_xyzh0011;
205 const float v_xyzh0001 = v_xyz000 * coords[3];
206 const float v_xyzh0000 = v_xyz000 - v_xyzh0001;
207
208 hists[grid_idx][h_idx] += v_xyzh0000;
209 hists[grid_idx][h_idx + 1] += v_xyzh0001;
210 hists[grid_idx + 1][h_idx] += v_xyzh0010;
211 hists[grid_idx + 1][h_idx + 1] += v_xyzh0011;
212 hists[grid_idx + (grid_size + 2)][h_idx] += v_xyzh0100;
213 hists[grid_idx + (grid_size + 2)][h_idx + 1] += v_xyzh0101;
214 hists[grid_idx + (grid_size + 3)][h_idx] += v_xyzh0110;
215 hists[grid_idx + (grid_size + 3)][h_idx + 1] += v_xyzh0111;
216 hists[grid_idx + (grid_size + 2) * (grid_size + 2)][h_idx] += v_xyzh1000;
217 hists[grid_idx + (grid_size + 2) * (grid_size + 2)][h_idx + 1] += v_xyzh1001;
218 hists[grid_idx + (grid_size + 2) * (grid_size + 2) + 1][h_idx] += v_xyzh1010;
219 hists[grid_idx + (grid_size + 2) * (grid_size + 2) + 1][h_idx + 1] += v_xyzh1011;
220 hists[grid_idx + (grid_size + 3) * (grid_size + 2)][h_idx] += v_xyzh1100;
221 hists[grid_idx + (grid_size + 3) * (grid_size + 2)][h_idx + 1] += v_xyzh1101;
222 hists[grid_idx + (grid_size + 3) * (grid_size + 2) + 1][h_idx] += v_xyzh1110;
223 hists[grid_idx + (grid_size + 3) * (grid_size + 2) + 1][h_idx + 1] += v_xyzh1111;
224 }
225 }
226}
227
228//////////////////////////////////////////////////////////////////////////////////////////////
229template <typename PointInT, typename PointOutT> void
231 const std::size_t hists_size,
232 const std::vector<Eigen::VectorXf> &hists,
233 PointCloudOut &output,
234 std::size_t &pos)
235{
236 for (std::size_t i = 0; i < grid_size; ++i)
237 {
238 for (std::size_t j = 0; j < grid_size; ++j)
239 {
240 for (std::size_t k = 0; k < grid_size; ++k)
241 {
242 const std::size_t idx = ( (i + 1) * (grid_size + 2) + (j + 1)) * (grid_size + 2) + (k + 1);
243
244 std::copy (hists[idx].data () + 1, hists[idx].data () + 1 + hists_size, output[0].histogram + pos);
245 pos += hists_size;
246 }
247 }
248 }
249}
250
251//////////////////////////////////////////////////////////////////////////////////////////////
252template <typename PointInT, typename PointOutT> void
254{
255 // compute alignment transform using reference frame
256 computeAlignmentTransform ();
257
258 // align point cloud
259 pcl::transformPointCloud (*surface_, *indices_, shape_samples_, transform_);
260
261 const std::size_t shape_grid_size = shape_half_grid_size_ * 2;
262
263 // each histogram dimension has 2 additional bins, 1 in each boundary, for performing interpolation
264 std::vector<Eigen::VectorXf> shape_hists ((shape_grid_size + 2) * (shape_grid_size + 2) * (shape_grid_size + 2),
265 Eigen::VectorXf::Zero (shape_hists_size_ + 2));
266
267 Eigen::Vector4f centroid_p = Eigen::Vector4f::Zero ();
268
269 // compute normalization factor for distances between samples and centroid
270 Eigen::Vector4f far_pt;
271 pcl::getMaxDistance (shape_samples_, centroid_p, far_pt);
272 far_pt[3] = 0;
273 const float distance_normalization_factor = (centroid_p - far_pt).norm ();
274
275 // compute normalization factor with respect to axis-aligned bounding cube centered on the origin
276 Eigen::Vector4f min_pt, max_pt;
277 pcl::getMinMax3D (shape_samples_, min_pt, max_pt);
278
279 max_coord_ = std::max (min_pt.head<3> ().cwiseAbs ().maxCoeff (), max_pt.head<3> ().cwiseAbs ().maxCoeff ());
280
281 // normalize sample contribution with respect to the total number of points in the cloud
282 hist_incr_ = 100.0f / static_cast<float> (shape_samples_.size () - 1);
283
284 // for each sample
285 for (const auto& sample: shape_samples_)
286 {
287 // compute shape histogram array coord based on distance between sample and centroid
288 const Eigen::Vector4f p (sample.x, sample.y, sample.z, 0.0f);
289 const float d = p.norm ();
290
291 const float shape_grid_step = distance_normalization_factor / shape_half_grid_size_;
292
293 float integral;
294 const float dist_hist_val = std::modf(d / shape_grid_step, &integral);
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[0].histogram + pos_, output[0].histogram + output[0].descriptorSize (), 0.0f);
309}
310
311//////////////////////////////////////////////////////////////////////////////////////////////
312template <typename PointInT, typename PointOutT> void
314 const std::size_t hists_size,
315 std::vector<Eigen::VectorXf> &hists,
316 PointCloudOut &output,
317 std::size_t &pos)
318{
319 for (std::size_t i = 0; i < grid_size; ++i)
320 {
321 for (std::size_t j = 0; j < grid_size; ++j)
322 {
323 for (std::size_t k = 0; k < grid_size; ++k)
324 {
325 const std::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 () + 1 + hists_size, output[0].histogram + pos);
331 pos += hists_size;
332 }
333 }
334 }
335}
336
337//////////////////////////////////////////////////////////////////////////////////////////////
338template <typename PointInT, typename PointOutT> void
340{
341 // call shape feature computation
342 GASDEstimation<PointInT, PointOutT>::computeFeature (output);
343
344 const std::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 (const auto& sample: shape_samples_)
352 {
353 // compute shape histogram array coord based on distance between sample and centroid
354 const Eigen::Vector4f p (sample.x, sample.y, sample.z, 0.0f);
355
356 // compute hue value
357 float hue = 0.f;
358
359 const unsigned char max = std::max (sample.r, std::max (sample.g, sample.b));
360 const unsigned char min = std::min (sample.r, std::min (sample.g, sample.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 == sample.r)
367 {
368 hue = 60.f * (static_cast <float> (sample.g - sample.b) * diff_inv);
369 }
370 else if (max == sample.g)
371 {
372 hue = 60.f * (2.f + static_cast <float> (sample.b - sample.r) * diff_inv);
373 }
374 else
375 {
376 hue = 60.f * (4.f + static_cast <float> (sample.r - sample.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[0].histogram + pos_, output[0].histogram + output[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_
Feature represents the base feature class.
Definition feature.h:107
GASDColorEstimation estimates the Globally Aligned Spatial Distribution (GASD) descriptor for a given...
Definition gasd.h:257
GASDEstimation estimates the Globally Aligned Spatial Distribution (GASD) descriptor for a given poin...
Definition gasd.h:75
void computeFeature(PointCloudOut &output) override
Estimate GASD descriptor.
Definition gasd.hpp:253
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
Definition gasd.hpp:50
void addSampleToHistograms(const Eigen::Vector4f &p, const float max_coord, const std::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:118
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t width
The point cloud width (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
std::uint32_t height
The point cloud height (if organized as an image-structure).
void clear()
Removes all points in a cloud and sets the width and height to 0.
Define standard C methods and C++ classes that are common to all methods.
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:197
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:295
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.
Definition centroid.hpp:192
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
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:295
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:57
HistogramInterpolationMethod
Different histogram interpolation methods.
Definition gasd.h:47
@ INTERP_NONE
no interpolation
Definition gasd.h:48
@ INTERP_TRILINEAR
trilinear interpolation
Definition gasd.h:49