41 #ifndef PCL_FEATURES_IMPL_CRH_H_
42 #define PCL_FEATURES_IMPL_CRH_H_
44 #include <pcl/features/crh.h>
45 #include <pcl/common/fft/kiss_fftr.h>
46 #include <pcl/common/common.h>
47 #include <pcl/common/transforms.h>
50 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
57 PCL_ERROR (
"[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
58 output.width = output.height = 0;
59 output.points.clear ();
63 if (normals_->points.size () != surface_->points.size ())
65 PCL_ERROR (
"[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the dataset containing the normals!\n", getClassName ().c_str ());
66 output.width = output.height = 0;
67 output.points.clear ();
71 Eigen::Vector3f plane_normal;
72 plane_normal[0] = -centroid_[0];
73 plane_normal[1] = -centroid_[1];
74 plane_normal[2] = -centroid_[2];
75 Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ ();
76 plane_normal.normalize ();
77 Eigen::Vector3f axis = plane_normal.cross (z_vector);
78 double rotation = -asin (axis.norm ());
82 int bin_angle = 360 / nbins;
84 Eigen::Affine3f transformPC (Eigen::AngleAxisf (static_cast<float> (rotation), axis));
87 grid.
points.resize (indices_->size ());
89 for (
size_t i = 0; i < indices_->size (); i++)
91 grid.
points[i].getVector4fMap () = surface_->points[(*indices_)[i]].getVector4fMap ();
92 grid.
points[i].getNormalVector4fMap () = normals_->points[(*indices_)[i]].getNormalVector4fMap ();
98 kiss_fft_scalar * spatial_data =
new kiss_fft_scalar[nbins];
99 float sum_w = 0, w = 0;
101 for (
size_t i = 0; i < grid.
points.size (); ++i)
103 bin =
static_cast<int> ((((atan2 (grid.
points[i].normal_y, grid.
points[i].normal_x) + M_PI) * 180 / M_PI) / bin_angle)) % nbins;
106 spatial_data[bin] += w;
109 for (
int i = 0; i < nbins; ++i)
110 spatial_data[i] /= sum_w;
113 kiss_fftr_cfg mycfg = kiss_fftr_alloc (nbins, 0, NULL, NULL);
114 kiss_fftr (mycfg, spatial_data, freq_data);
116 output.points.resize (1);
117 output.width = output.height = 1;
119 output.points[0].histogram[0] = freq_data[0].
r / freq_data[0].
r;
121 for (
int i = 1; i < (nbins / 2); i++, k += 2)
123 output.points[0].histogram[k] = freq_data[i].
r / freq_data[0].
r;
124 output.points[0].histogram[k + 1] = freq_data[i].
i / freq_data[0].
r;
127 output.points[0].histogram[nbins - 1] = freq_data[nbins / 2].
r / freq_data[0].
r;
129 delete[] spatial_data;
134 #define PCL_INSTANTIATE_CRHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CRHEstimation<T,NT,OutT>;
136 #endif // PCL_FEATURES_IMPL_CRH_H_
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void transformPointCloudWithNormals(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform)
Transform a point cloud and rotate its normals using an Eigen transform.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
CRHEstimation estimates the Camera Roll Histogram (CRH) descriptor for a given point cloud dataset co...