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 (std::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 std::vector<kiss_fft_scalar> spatial_data(nbins);
102 for (
const auto &point : grid.
points)
104 int bin =
static_cast<int> ((((std::atan2 (point.normal_y, point.normal_x) + M_PI) * 180 / M_PI) / bin_angle)) % nbins;
105 float w = std::sqrt (point.normal_y * point.normal_y + point.normal_x * point.normal_x);
107 spatial_data[bin] += w;
110 for (
auto& data: spatial_data)
113 std::vector<kiss_fft_cpx> freq_data(nbins / 2 + 1);
114 kiss_fftr_cfg mycfg = kiss_fftr_alloc (nbins, 0,
nullptr,
nullptr);
115 kiss_fftr (mycfg, spatial_data.data (), freq_data.data ());
117 for (
auto& data: freq_data)
119 data.r /= freq_data[0].r;
120 data.i /= freq_data[0].r;
123 output.points.resize (1);
124 output.width = output.height = 1;
126 output.points[0].histogram[0] = freq_data[0].r;
128 for (
int i = 1; i < (nbins / 2); i++, k += 2)
130 output.points[0].histogram[k] = freq_data[i].r;
131 output.points[0].histogram[k + 1] = freq_data[i].i;
134 output.points[0].histogram[nbins - 1] = freq_data[nbins / 2].r;
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)
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...