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 ();
99 kiss_fft_scalar * spatial_data =
new kiss_fft_scalar[nbins]();
102 float sum_w = 0, w = 0;
104 for (
size_t i = 0; i < grid.
points.size (); ++i)
106 bin =
static_cast<int> ((((atan2 (grid.
points[i].normal_y, grid.
points[i].normal_x) + M_PI) * 180 / M_PI) / bin_angle)) % nbins;
109 spatial_data[bin] += w;
112 for (
int i = 0; i < nbins; ++i)
113 spatial_data[i] /= sum_w;
116 kiss_fftr_cfg mycfg = kiss_fftr_alloc (nbins, 0, NULL, NULL);
117 kiss_fftr (mycfg, spatial_data, freq_data);
119 output.points.resize (1);
120 output.width = output.height = 1;
122 output.points[0].histogram[0] = freq_data[0].
r / freq_data[0].
r;
124 for (
int i = 1; i < (nbins / 2); i++, k += 2)
126 output.points[0].histogram[k] = freq_data[i].
r / freq_data[0].
r;
127 output.points[0].histogram[k + 1] = freq_data[i].
i / freq_data[0].
r;
130 output.points[0].histogram[nbins - 1] = freq_data[nbins / 2].
r / freq_data[0].
r;
132 delete[] spatial_data;
137 #define PCL_INSTANTIATE_CRHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CRHEstimation<T,NT,OutT>; 139 #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, bool copy_all_fields=true)
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...