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 std::size_t half_grid_size,
122 const float hist_incr,
123 std::vector<Eigen::VectorXf> &hists)
125 const std::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 std::size_t grid_idx = ( (bins[0] + 1) * (grid_size + 2) + bins[1] + 1) * (grid_size + 2) + bins[2] + 1;
144 const std::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 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.points[0].histogram + pos);
251 template <
typename Po
intInT,
typename Po
intOutT>
void
255 computeAlignmentTransform ();
260 const std::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 (std::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_;
293 const float dist_hist_val = std::modf(d / shape_grid_step, &integral);
295 const float dbin = dist_hist_val * shape_hists_size_;
298 addSampleToHistograms (p, max_coord_, shape_half_grid_size_, shape_interp_, dbin, hist_incr_, shape_hists);
304 copyShapeHistogramsToOutput (shape_grid_size, shape_hists_size_, shape_hists, output, pos_);
307 std::fill (output.
points[0].histogram + pos_, output.
points[0].histogram + output.
points[0].descriptorSize (), 0.0f);
311 template <
typename Po
intInT,
typename Po
intOutT>
void
313 const std::size_t hists_size,
314 std::vector<Eigen::VectorXf> &hists,
315 PointCloudOut &output,
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);
326 hists[idx][1] += hists[idx][hists_size + 1];
327 hists[idx][hists_size] += hists[idx][0];
329 std::copy (hists[idx].data () + 1, hists[idx].data () + hists_size + 1, output.points[0].histogram + pos);
337 template <
typename Po
intInT,
typename Po
intOutT>
void
341 GASDEstimation<PointInT, PointOutT>::computeFeature (output);
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 (std::size_t i = 0; i < shape_samples_.size (); ++i)
353 const Eigen::Vector4f p (shape_samples_[i].x, shape_samples_[i].y, shape_samples_[i].z, 0.0f);
358 const unsigned char max = std::max (shape_samples_[i].r, std::max (shape_samples_[i].g, shape_samples_[i].b));
359 const unsigned char min = std::min (shape_samples_[i].r, std::min (shape_samples_[i].g, shape_samples_[i].b));
361 const float diff_inv = 1.f / static_cast <float> (max - min);
363 if (std::isfinite (diff_inv))
365 if (max == shape_samples_[i].r)
367 hue = 60.f * (static_cast <float> (shape_samples_[i].g - shape_samples_[i].b) * diff_inv);
369 else if (max == shape_samples_[i].g)
371 hue = 60.f * (2.f + static_cast <float> (shape_samples_[i].b - shape_samples_[i].r) * diff_inv);
375 hue = 60.f * (4.f + static_cast <float> (shape_samples_[i].r - shape_samples_[i].g) * diff_inv);
385 const float hbin = (hue / 360) * color_hists_size_;
388 GASDEstimation<PointInT, PointOutT>::addSampleToHistograms (p, max_coord_, color_half_grid_size_, color_interp_, hbin, hist_incr_, color_hists);
392 copyColorHistogramsToOutput (color_grid_size, color_hists_size_, color_hists, output, pos_);
395 std::fill (output.points[0].histogram + pos_, output.points[0].histogram + output.points[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>;
401 #endif // PCL_FEATURES_IMPL_GASD_H_