template <typename PointInT, typename PointNT> void
pcl::BoundaryEstimation<PointInT, PointNT, Eigen::MatrixXf>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
{
// Allocate enough space to hold the results
// \note This resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices (k_);
std::vector<float> nn_dists (k_);
Eigen::Vector4f u = Eigen::Vector4f::Zero (), v = Eigen::Vector4f::Zero ();
output.is_dense = true;
output.points.resize (indices_->size (), 1);
// Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense
if (input_->is_dense)
{
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
output.points (idx, 0) = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
// Obtain a coordinate system on the least-squares plane
//v = normals_->points[(*indices_)[idx]].getNormalVector4fMap ().unitOrthogonal ();
//u = normals_->points[(*indices_)[idx]].getNormalVector4fMap ().cross3 (v);
this->getCoordinateSystemOnPlane (normals_->points[(*indices_)[idx]], u, v);
// Estimate whether the point is lying on a boundary surface or not
output.points (idx, 0) = this->isBoundaryPoint (*surface_, input_->points[(*indices_)[idx]], nn_indices, u, v, angle_threshold_);
}
}
else
{
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (!isFinite ((*input_)[(*indices_)[idx]]) ||
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
output.points (idx, 0) = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
// Obtain a coordinate system on the least-squares plane
//v = normals_->points[(*indices_)[idx]].getNormalVector4fMap ().unitOrthogonal ();
//u = normals_->points[(*indices_)[idx]].getNormalVector4fMap ().cross3 (v);
this->getCoordinateSystemOnPlane (normals_->points[(*indices_)[idx]], u, v);
// Estimate whether the point is lying on a boundary surface or not
output.points (idx, 0) = this->isBoundaryPoint (*surface_, input_->points[(*indices_)[idx]], nn_indices, u, v, angle_threshold_);
}
}
}
template <typename PointInT, typename PointNT> void
pcl::PrincipalCurvaturesEstimation<PointInT, PointNT, Eigen::MatrixXf>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
{
// Resize the output dataset
output.points.resize (indices_->size (), 5);
// Allocate enough space to hold the results
// \note This resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices (k_);
std::vector<float> nn_dists (k_);
output.is_dense = true;
// Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense
if (input_->is_dense)
{
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
output.points.row (idx).setConstant (std::numeric_limits<float>::quiet_NaN ());
output.is_dense = false;
continue;
}
// Estimate the principal curvatures at each patch
this->computePointPrincipalCurvatures (*normals_, (*indices_)[idx], nn_indices,
output.points (idx, 0), output.points (idx, 1), output.points (idx, 2),
output.points (idx, 3), output.points (idx, 4));
}
}
else
{
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (!isFinite ((*input_)[(*indices_)[idx]]) ||
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
output.points.row (idx).setConstant (std::numeric_limits<float>::quiet_NaN ());
output.is_dense = false;
continue;
}
// Estimate the principal curvatures at each patch
this->computePointPrincipalCurvatures (*normals_, (*indices_)[idx], nn_indices,
output.points (idx, 0), output.points (idx, 1), output.points (idx, 2),
output.points (idx, 3), output.points (idx, 4));
}
}
}
template <typename PointInT, typename PointNT> void
pcl::IntensityGradientEstimation<PointInT, PointNT, Eigen::MatrixXf>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
{
// Resize the output dataset
output.points.resize (indices_->size (), 3);
// Allocate enough space to hold the results
// \note This resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices (k_);
std::vector<float> nn_dists (k_);
output.is_dense = true;
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
output.points.row (idx).setConstant (std::numeric_limits<float>::quiet_NaN ());
output.is_dense = false;
continue;
}
Eigen::Vector4f centroid;
compute3DCentroid (*surface_, nn_indices, centroid);
float mean_intensity = 0;
unsigned valid_neighbor_count = 0;
for (size_t nIdx = 0; nIdx < nn_indices.size (); ++nIdx)
{
const PointInT& p = (*surface_)[nn_indices[nIdx]];
if (!pcl_isfinite (p.intensity))
continue;
mean_intensity += p.intensity;
++valid_neighbor_count;
}
mean_intensity /= static_cast<float> (valid_neighbor_count);
Eigen::Vector3f normal = Eigen::Vector3f::Map (normals_->points[idx].normal);
Eigen::Vector3f gradient;
this->computePointIntensityGradient (*surface_, nn_indices, centroid.head<3> (), mean_intensity, normal, gradient);
output.points (idx, 0) = gradient[0];
output.points (idx, 1) = gradient[1];
output.points (idx, 2) = gradient[2];
}
}
template <typename PointInT, typename PointRFT> void
pcl::UniqueShapeContext<PointInT, Eigen::MatrixXf, PointRFT>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
{
output.points.resize (indices_->size (), descriptor_length_ + 9);
for (size_t point_index = 0; point_index < indices_->size (); point_index++)
{
for (int d = 0; d < 9; ++d)
output.points (point_index, d) = frames_->points[point_index].rf[ (4*(d/3) + (d%3)) ];
std::vector<float> descriptor (descriptor_length_);
computePointDescriptor (point_index, descriptor);
for (size_t j = 0; j < descriptor_length_; ++j)
output.points (point_index, 9 + j) = descriptor[j];
}
}
template <typename PointInT, typename PointNT> void
pcl::ShapeContext3DEstimation<PointInT, PointNT, Eigen::MatrixXf>::computeFeatureEigen (
pcl::PointCloud<Eigen::MatrixXf> &output)
{
// Set up the output channels
output.channels["3dsc"].name = "3dsc";
output.channels["3dsc"].offset = 0;
output.channels["3dsc"].size = 4;
output.channels["3dsc"].count = descriptor_length_ + 9;
output.channels["3dsc"].datatype = sensor_msgs::PointField::FLOAT32;
// Resize the output dataset
output.points.resize (indices_->size (), descriptor_length_ + 9);
float rf[9];
output.is_dense = true;
// Iterate over all points and compute the descriptors
for (size_t point_index = 0; point_index < indices_->size (); point_index++)
{
// If the point is not finite, set the descriptor to NaN and continue
if (!isFinite ((*input_)[(*indices_)[point_index]]))
{
output.points.row (point_index).setConstant (std::numeric_limits<float>::quiet_NaN ());
output.is_dense = false;
continue;
}
std::vector<float> descriptor (descriptor_length_);
if (!this->computePoint (point_index, *normals_, rf, descriptor))
output.is_dense = false;
for (int j = 0; j < 9; ++j)
output.points (point_index, j) = rf[j];
for (size_t j = 0; j < descriptor_length_; ++j)
output.points (point_index, 9 + j) = descriptor[j];
}
}
template <typename PointInT> void
pcl::NormalEstimation<PointInT, Eigen::MatrixXf>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
{
// Resize the output dataset
output.points.resize (indices_->size (), 4);
// Allocate enough space to hold the results
// \note This resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices (k_);
std::vector<float> nn_dists (k_);
output.is_dense = true;
// Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense
if (input_->is_dense)
{
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
output.points (idx, 0) = output.points (idx, 1) = output.points (idx, 2) = output.points (idx, 3) = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
computePointNormal (*surface_, nn_indices,
output.points (idx, 0), output.points (idx, 1), output.points (idx, 2), output.points (idx, 3));
flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_,
output.points (idx, 0), output.points (idx, 1), output.points (idx, 2));
}
}
else
{
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (!isFinite ((*input_)[(*indices_)[idx]]) ||
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
output.points (idx, 0) = output.points (idx, 1) = output.points (idx, 2) = output.points (idx, 3) = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
computePointNormal (*surface_, nn_indices,
output.points (idx, 0), output.points (idx, 1), output.points (idx, 2), output.points (idx, 3));
flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_,
output.points (idx, 0), output.points (idx, 1), output.points (idx, 2));
}
}
}
template <typename PointInT, typename GradientT> void
pcl::RIFTEstimation<PointInT, GradientT, Eigen::MatrixXf>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
{
// These should be moved into initCompute ()
{
// Make sure a search radius is set
if (search_radius_ == 0.0)
{
PCL_ERROR ("[pcl::%s::computeFeature] The search radius must be set before computing the feature!\n",
getClassName ().c_str ());
output.width = output.height = 0;
output.points.resize (0, 0);
return;
}
// Make sure the RIFT descriptor has valid dimensions
if (nr_gradient_bins_ <= 0)
{
PCL_ERROR ("[pcl::%s::computeFeature] The number of gradient bins must be greater than zero!\n",
getClassName ().c_str ());
output.width = output.height = 0;
output.points.resize (0, 0);
return;
}
if (nr_distance_bins_ <= 0)
{
PCL_ERROR ("[pcl::%s::computeFeature] The number of distance bins must be greater than zero!\n",
getClassName ().c_str ());
output.width = output.height = 0;
output.points.resize (0, 0);
return;
}
// Check for valid input gradient
if (!gradient_)
{
PCL_ERROR ("[pcl::%s::computeFeature] No input gradient was given!\n", getClassName ().c_str ());
output.width = output.height = 0;
output.points.resize (0, 0);
return;
}
if (gradient_->points.size () != surface_->points.size ())
{
PCL_ERROR ("[pcl::%s::computeFeature] ", getClassName ().c_str ());
PCL_ERROR ("The number of points in the input dataset differs from the number of points in the gradient!\n");
output.width = output.height = 0;
output.points.resize (0, 0);
return;
}
}
output.points.resize (indices_->size (), nr_gradient_bins_ * nr_distance_bins_);
Eigen::MatrixXf rift_descriptor (nr_distance_bins_, nr_gradient_bins_);
std::vector<int> nn_indices;
std::vector<float> nn_dist_sqr;
output.is_dense = true;
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
// Find neighbors within the search radius
if (tree_->radiusSearch ((*indices_)[idx], search_radius_, nn_indices, nn_dist_sqr) == 0)
{
output.points.row (idx).setConstant (std::numeric_limits<float>::quiet_NaN ());
output.is_dense = false;
continue;
}
// Compute the RIFT descriptor
this->computeRIFT (*surface_, *gradient_, (*indices_)[idx], static_cast<float> (search_radius_), nn_indices, nn_dist_sqr,
rift_descriptor);
// Copy into the resultant cloud
int bin = 0;
for (int g_bin = 0; g_bin < rift_descriptor.cols (); ++g_bin)
for (int d_bin = 0; d_bin < rift_descriptor.rows (); ++d_bin)
output.points (idx, bin++) = rift_descriptor (d_bin, g_bin);
}
}
