39 #ifndef PCL_FEATURES_IMPL_GASD_H_
40 #define PCL_FEATURES_IMPL_GASD_H_
42 #include <pcl/features/gasd.h>
43 #include <pcl/common/transforms.h>
44 #include <pcl/point_types_conversion.h>
49 template <
typename Po
intInT,
typename Po
intOutT>
void
63 output.
header = surface_->header;
64 output.
is_dense = surface_->is_dense;
67 computeFeature (output);
73 template <
typename Po
intInT,
typename Po
intOutT>
void
76 Eigen::Vector4f centroid;
77 Eigen::Matrix3f covariance_matrix;
85 Eigen::Matrix3f eigenvectors;
86 Eigen::Vector3f eigenvalues;
89 pcl::eigen33 (covariance_matrix, eigenvectors, eigenvalues);
92 Eigen::Vector3f z_axis = eigenvectors.col (0);
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;
116 template <
typename Po
intInT,
typename Po
intOutT>
void
118 const float max_coord,
119 const size_t half_grid_size,
122 const float hist_incr,
123 std::vector<Eigen::VectorXf> &hists)
125 const size_t grid_size = half_grid_size * 2;
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);
131 Eigen::Vector4f coords (scaled[0] + half_grid_size, scaled[1] + half_grid_size, scaled[2] + half_grid_size, hbin);
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]));
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;
149 hists[grid_idx][h_idx] += hist_incr;
154 coords -= Eigen::Vector4f (bins[0], bins[1], bins[2], 0.0f);
156 const float v_x1 = hist_incr * coords[0];
157 const float v_x0 = hist_incr - v_x1;
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;
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;
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;
188 coords[3] -= bins[3];
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;
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;
228 template <
typename Po
intInT,
typename Po
intOutT>
void
230 const size_t hists_size,
231 const std::vector<Eigen::VectorXf> &hists,
232 PointCloudOut &output,
235 for (
size_t i = 0; i < grid_size; ++i)
237 for (
size_t j = 0; j < grid_size; ++j)
239 for (
size_t k = 0; k < grid_size; ++k)
241 const 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.points[0].histogram + pos);
251 template <
typename Po
intInT,
typename Po
intOutT>
void
255 computeAlignmentTransform ();
260 const size_t shape_grid_size = shape_half_grid_size_ * 2;
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));
266 Eigen::Vector4f centroid_p = Eigen::Vector4f::Zero ();
269 Eigen::Vector4f far_pt;
272 const float distance_normalization_factor = (centroid_p - far_pt).norm ();
275 Eigen::Vector4f min_pt, max_pt;
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 (
size_t i = 0; i < shape_samples_.size (); ++i)
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 ();
290 const float shape_grid_step = distance_normalization_factor / shape_half_grid_size_;
292 const float dist_hist_start = ((int) (d / shape_grid_step)) * shape_grid_step;
294 const float dist_hist_val = (d - dist_hist_start) / shape_grid_step;
296 const float dbin = dist_hist_val * shape_hists_size_;
299 addSampleToHistograms (p, max_coord_, shape_half_grid_size_, shape_interp_, dbin, hist_incr_, shape_hists);
305 copyShapeHistogramsToOutput (shape_grid_size, shape_hists_size_, shape_hists, output, pos_);
308 std::fill (output.
points[0].histogram + pos_, output.
points[0].histogram + output.
points[0].descriptorSize (), 0.0f);
312 template <
typename Po
intInT,
typename Po
intOutT>
void
314 const size_t hists_size,
315 std::vector<Eigen::VectorXf> &hists,
316 PointCloudOut &output,
319 for (
size_t i = 0; i < grid_size; ++i)
321 for (
size_t j = 0; j < grid_size; ++j)
323 for (
size_t k = 0; k < grid_size; ++k)
325 const size_t idx = ( (i + 1) * (grid_size + 2) + (j + 1)) * (grid_size + 2) + (k + 1);
327 hists[idx][1] += hists[idx][hists_size + 1];
328 hists[idx][hists_size] += hists[idx][0];
330 std::copy (hists[idx].data () + 1, hists[idx].data () + hists_size + 1, output.points[0].histogram + pos);
338 template <
typename Po
intInT,
typename Po
intOutT>
void
342 GASDEstimation<PointInT, PointOutT>::computeFeature (output);
344 const size_t color_grid_size = color_half_grid_size_ * 2;
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));
351 for (
size_t i = 0; i < shape_samples_.size (); ++i)
354 const Eigen::Vector4f p (shape_samples_[i].x, shape_samples_[i].y, shape_samples_[i].z, 0.0f);
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));
362 const float diff_inv = 1.f / static_cast <
float> (max - min);
364 if (pcl_isfinite (diff_inv))
366 if (max == shape_samples_[i].r)
368 hue = 60.f * (static_cast <
float> (shape_samples_[i].g - shape_samples_[i].b) * diff_inv);
370 else if (max == shape_samples_[i].g)
372 hue = 60.f * (2.f + static_cast <
float> (shape_samples_[i].b - shape_samples_[i].r) * diff_inv);
376 hue = 60.f * (4.f + static_cast <
float> (shape_samples_[i].r - shape_samples_[i].g) * diff_inv);
386 const float hbin = (hue / 360) * color_hists_size_;
389 GASDEstimation<PointInT, PointOutT>::addSampleToHistograms (p, max_coord_, color_half_grid_size_, color_interp_, hbin, hist_incr_, color_hists);
393 copyColorHistogramsToOutput (color_grid_size, color_hists_size_, color_hists, output, pos_);
396 std::fill (output.points[0].histogram + pos_, output.points[0].histogram + output.points[0].descriptorSize (), 0.0f);
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>;
402 #endif // PCL_FEATURES_IMPL_GASD_H_
uint32_t width
The point cloud width (if organized as an image-structure).
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
GASDColorEstimation estimates the Globally Aligned Spatial Distribution (GASD) descriptor for a given...
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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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.
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 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...
uint32_t height
The point cloud height (if organized as an image-structure).
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...
HistogramInterpolationMethod
Different histogram interpolation methods.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void computeFeature(PointCloudOut &output)
Estimate GASD descriptor.
GASDEstimation estimates the Globally Aligned Spatial Distribution (GASD) descriptor for a given poin...
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.
Feature represents the base feature class.
void resize(size_t n)
Resize the cloud.
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.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
pcl::PCLHeader header
The point cloud header.