39#ifndef PCL_FEATURES_IMPL_GASD_H_
40#define PCL_FEATURES_IMPL_GASD_H_
42#include <pcl/features/gasd.h>
44#include <pcl/common/transforms.h>
49template <
typename Po
intInT,
typename Po
intOutT>
void
63 output.header = surface_->header;
64 output.is_dense = surface_->is_dense;
73template <
typename Po
intInT,
typename Po
intOutT>
void
76 Eigen::Vector4f centroid;
95 if (z_axis.dot (view_direction_) > 0)
101 const Eigen::Vector3f x_axis = eigenvectors.col (2);
104 const Eigen::Vector3f y_axis = z_axis.cross (x_axis);
106 const Eigen::Vector3f centroid_xyz = centroid.head<3> ();
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;
116template <
typename Po
intInT,
typename Po
intOutT>
void
123 std::vector<Eigen::VectorXf> &
hists)
136 coords -= Eigen::Vector4f (0.5f, 0.5f, 0.5f, 0.5f);
140 const Eigen::Vector4f
bins (std::floor (coords[0]), std::floor (coords[1]), std::floor (coords[2]), std::floor (coords[3]));
154 coords -= Eigen::Vector4f (
bins[0],
bins[1],
bins[2], 0.0f);
188 coords[3] -=
bins[3];
228template <
typename Po
intInT,
typename Po
intOutT>
void
230 const std::size_t hists_size,
231 const std::vector<Eigen::VectorXf> &hists,
232 PointCloudOut &output,
235 for (std::size_t i = 0; i < grid_size; ++i)
237 for (std::size_t j = 0; j < grid_size; ++j)
239 for (std::size_t k = 0; k < grid_size; ++k)
241 const std::size_t idx = ( (i + 1) * (grid_size + 2) + (j + 1)) * (grid_size + 2) + (k + 1);
243 std::copy (hists[idx].data () + 1, hists[idx].data () + hists_size + 1, output[0].histogram + pos);
251template <
typename Po
intInT,
typename Po
intOutT>
void
255 computeAlignmentTransform ();
264 Eigen::VectorXf::Zero (shape_hists_size_ + 2));
266 Eigen::Vector4f
centroid_p = Eigen::Vector4f::Zero ();
278 max_coord_ = std::max (
min_pt.head<3> ().cwiseAbs ().maxCoeff (),
max_pt.head<3> ().cwiseAbs ().maxCoeff ());
281 hist_incr_ = 100.0f /
static_cast<float> (shape_samples_.size () - 1);
284 for (
const auto& sample: shape_samples_)
287 const Eigen::Vector4f p (sample.x, sample.y, sample.z, 0.0f);
288 const float d = p.norm ();
298 addSampleToHistograms (p, max_coord_, shape_half_grid_size_, shape_interp_,
dbin, hist_incr_,
shape_hists);
307 std::fill (
output[0].histogram + pos_,
output[0].histogram +
output[0].descriptorSize (), 0.0f);
311template <
typename Po
intInT,
typename Po
intOutT>
void
314 std::vector<Eigen::VectorXf> &
hists,
318 for (std::size_t i = 0; i <
grid_size; ++i)
320 for (std::size_t j = 0; j <
grid_size; ++j)
322 for (std::size_t k = 0; k <
grid_size; ++k)
324 const std::size_t idx = ( (i + 1) * (
grid_size + 2) + (j + 1)) * (
grid_size + 2) + (k + 1);
337template <
typename Po
intInT,
typename Po
intOutT>
void
343 const std::size_t color_grid_size = color_half_grid_size_ * 2;
346 std::vector<Eigen::VectorXf> color_hists ((color_grid_size + 2) * (color_grid_size + 2) * (color_grid_size + 2),
347 Eigen::VectorXf::Zero (color_hists_size_ + 2));
350 for (
const auto& sample: shape_samples_)
353 const Eigen::Vector4f p (sample.x, sample.y, sample.z, 0.0f);
358 const unsigned char max = std::max (sample.r, std::max (sample.g, sample.b));
359 const unsigned char min = std::min (sample.r, std::min (sample.g, sample.b));
361 const float diff_inv = 1.f /
static_cast <float> (max - min);
363 if (std::isfinite (diff_inv))
367 hue = 60.f * (
static_cast <float> (sample.g - sample.b) * diff_inv);
369 else if (max == sample.g)
371 hue = 60.f * (2.f +
static_cast <float> (sample.b - sample.r) * diff_inv);
375 hue = 60.f * (4.f +
static_cast <float> (sample.r - sample.g) * diff_inv);
385 const float hbin = (hue / 360) * color_hists_size_;
392 copyColorHistogramsToOutput (color_grid_size, color_hists_size_, color_hists, output, pos_);
395 std::fill (output[0].histogram + pos_, output[0].histogram + output[0].descriptorSize (), 0.0f);
398#define PCL_INSTANTIATE_GASDEstimation(InT, OutT) template class PCL_EXPORTS pcl::GASDEstimation<InT, OutT>;
399#define PCL_INSTANTIATE_GASDColorEstimation(InT, OutT) template class PCL_EXPORTS pcl::GASDColorEstimation<InT, OutT>;
Iterator class for point clouds with or without given indices.
Feature represents the base feature class.
virtual bool deinitCompute()
This method should get called after ending the actual computation.
GASDColorEstimation estimates the Globally Aligned Spatial Distribution (GASD) descriptor for a given...
GASDEstimation estimates the Globally Aligned Spatial Distribution (GASD) descriptor for a given poin...
void computeFeature(PointCloudOut &output) override
Estimate GASD descriptor.
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
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.
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.
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.
unsigned int computeCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > ¢roid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute the 3x3 covariance matrix of a given set of points.
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...
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
HistogramInterpolationMethod
Different histogram interpolation methods.
@ INTERP_NONE
no interpolation
@ INTERP_TRILINEAR
trilinear interpolation