template <typename PointInT, typename PointOutT> void
pcl::TBB_NormalEstimationTBB<PointInT, PointOutT>::operator () (const tbb::blocked_range <size_t> &r) const
{
float vpx, vpy, vpz;
feature_->getViewPoint (vpx, vpy, vpz);
// Iterating over the entire index vector
for (size_t idx = r.begin (); idx != r.end (); ++idx)
{
std::vector<int> nn_indices (feature_->getKSearch ());
std::vector<float> nn_dists (feature_->getKSearch ());
feature_->searchForNeighbors ((*feature_->getIndices ())[idx], feature_->getSearchParameter (), nn_indices, nn_dists);
// 16-bytes aligned placeholder for the XYZ centroid of a surface patch
Eigen::Vector4f xyz_centroid;
// Estimate the XYZ centroid
compute3DCentroid (*feature_->getSearchSurface (), nn_indices, xyz_centroid);
// Placeholder for the 3x3 covariance matrix at each surface patch
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
// Compute the 3x3 covariance matrix
computeCovarianceMatrix (*feature_->getSearchSurface (), nn_indices, xyz_centroid, covariance_matrix);
// Get the plane normal and surface curvature
solvePlaneParameters (covariance_matrix,
output_.points[idx].normal[0], output_.points[idx].normal[1], output_.points[idx].normal[2], output_.points[idx].curvature);
flipNormalTowardsViewpoint<PointInT> (feature_->getSearchSurface ()->points[idx], vpx, vpy, vpz,
output_.points[idx].normal[0], output_.points[idx].normal[1], output_.points[idx].normal[2]);
}
}
template <typename PointInT, typename PointNT> void
pcl::PFHEstimation<PointInT, PointNT, Eigen::MatrixXf>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
{
// Set up the output channels
output.channels["pfh"].name = "pfh";
output.channels["pfh"].offset = 0;
output.channels["pfh"].size = 4;
output.channels["pfh"].count = nr_subdiv_ * nr_subdiv_ * nr_subdiv_;
output.channels["pfh"].datatype = sensor_msgs::PointField::FLOAT32;
// Clear the feature map
feature_map_.clear ();
std::queue<std::pair<int, int> > empty;
std::swap (key_list_, empty);
pfh_histogram_.setZero (nr_subdiv_ * nr_subdiv_ * nr_subdiv_);
// Allocate enough space to hold the results
output.points.resize (indices_->size (), nr_subdiv_ * nr_subdiv_ * nr_subdiv_);
// \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 PFH signature at each patch
computePointPFHSignature (*surface_, *normals_, nn_indices, nr_subdiv_, pfh_histogram_);
output.points.row (idx) = pfh_histogram_;
}
}
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 PFH signature at each patch
computePointPFHSignature (*surface_, *normals_, nn_indices, nr_subdiv_, pfh_histogram_);
output.points.row (idx) = pfh_histogram_;
}
}
}
double ORPointCloud::computeCloudRMS(const ORPointCloud* target, pcl::PointCloud<PointType>::ConstPtr source, double max_range){
pcl::search::KdTree<PointType>::Ptr tree(new pcl::search::KdTree<PointType>);
tree->setInputCloud(target->cloud);
double fitness_score = 0.0;
std::vector<int> nn_indices (1);
std::vector<float> nn_dists (1);
// For each point in the source dataset
int nr = 0;
for (size_t i = 0; i < source->points.size (); ++i){
//Avoid NaN points as they crash nn searches
if(!pcl_isfinite((*source)[i].x)){
continue;
}
// Find its nearest neighbor in the target
tree->nearestKSearch (source->points[i], 1, nn_indices, nn_dists);
// Deal with occlusions (incomplete targets)
if (nn_dists[0] <= max_range*max_range){
// Add to the fitness score
fitness_score += nn_dists[0];
nr++;
}
}
if (nr > 0){
return sqrt(fitness_score / nr);
}else{
return (std::numeric_limits<double>::max ());
}
}
template <typename PointInT, typename PointOutT> void
pcl::NormalEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &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_);
// 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))
{
output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits<float>::quiet_NaN ();
continue;
}
computePointNormal (*surface_, nn_indices,
output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature);
flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_,
output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]);
}
}
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::PFHRGBEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
/// nr_subdiv^3 for RGB and nr_subdiv^3 for the angular features
pfhrgb_histogram_.setZero (2 * nr_subdiv_ * nr_subdiv_ * nr_subdiv_);
pfhrgb_tuple_.setZero (7);
// 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_);
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists);
// Estimate the PFH signature at each patch
computePointPFHRGBSignature (*surface_, *normals_, nn_indices, nr_subdiv_, pfhrgb_histogram_);
// Copy into the resultant cloud
for (int d = 0; d < pfhrgb_histogram_.size (); ++d) {
output.points[idx].histogram[d] = pfhrgb_histogram_[d];
// PCL_INFO ("%f ", pfhrgb_histogram_[d]);
}
// PCL_INFO ("\n");
}
}
void
pcl::visualization::getCorrespondingPointCloud (vtkPolyData *src,
const pcl::PointCloud<pcl::PointXYZ> &tgt,
std::vector<int> &indices)
{
// Iterate through the points and copy the data in a pcl::PointCloud
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.height = 1; cloud.width = src->GetNumberOfPoints ();
cloud.is_dense = false;
cloud.points.resize (cloud.width * cloud.height);
for (int i = 0; i < src->GetNumberOfPoints (); i++)
{
double p[3];
src->GetPoint (i, p);
cloud.points[i].x = p[0]; cloud.points[i].y = p[1]; cloud.points[i].z = p[2];
}
// Compute a kd-tree for tgt
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > tgt_ptr (new pcl::PointCloud<pcl::PointXYZ> (tgt));
kdtree.setInputCloud (tgt_ptr);
std::vector<int> nn_indices (1);
std::vector<float> nn_dists (1);
// For each point on screen, find its correspondent in the target
for (size_t i = 0; i < cloud.points.size (); ++i)
{
kdtree.nearestKSearch (cloud.points[i], 1, nn_indices, nn_dists);
indices.push_back (nn_indices[0]);
}
// Sort and remove duplicate indices
std::sort (indices.begin (), indices.end ());
indices.erase (std::unique (indices.begin (), indices.end ()), indices.end ());
}
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
// Check if input was set
if (!normals_)
{
ROS_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!", getClassName ().c_str ());
output.width = output.height = 0;
output.points.clear ();
return;
}
if (normals_->points.size () != surface_->points.size ())
{
ROS_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the dataset containing the normals!", getClassName ().c_str ());
output.width = output.height = 0;
output.points.clear ();
return;
}
// 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_);
size_t data_size = indices_->size ();
// Reset the whole thing
hist_f1_.setZero (data_size, nr_bins_f1_);
hist_f2_.setZero (data_size, nr_bins_f2_);
hist_f3_.setZero (data_size, nr_bins_f3_);
int nr_bins = nr_bins_f1_ + nr_bins_f2_ + nr_bins_f3_;
// Iterating over the entire index vector
for (size_t idx = 0; idx < data_size; ++idx)
{
int p_idx = (*indices_)[idx];
searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists);
// Estimate the FPFH signature at each patch
computePointSPFHSignature (*surface_, *normals_, p_idx, nn_indices,
hist_f1_, hist_f2_, hist_f3_);
}
fpfh_histogram_.setZero (nr_bins);
// Iterating over the entire index vector
for (size_t idx = 0; idx < data_size; ++idx)
{
searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists);
weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_);
// Copy into the resultant cloud
for (int d = 0; d < fpfh_histogram_.size (); ++d)
output.points[idx].histogram[d] = fpfh_histogram_[d];
fpfh_histogram_.setZero ();
}
}
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computeSPFHSignatures (std::vector<int> &spfh_hist_lookup,
Eigen::MatrixXf &hist_f1, Eigen::MatrixXf &hist_f2, Eigen::MatrixXf &hist_f3)
{
// Allocate enough space to hold the NN search results
// \note This resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices (k_);
std::vector<float> nn_dists (k_);
std::set<int> spfh_indices;
spfh_hist_lookup.resize (surface_->points.size ());
// Build a list of (unique) indices for which we will need to compute SPFH signatures
// (We need an SPFH signature for every point that is a neighbor of any point in input_[indices_])
if (surface_ != input_ ||
indices_->size () != surface_->points.size ())
{
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
int p_idx = (*indices_)[idx];
if (this->searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists) == 0)
continue;
spfh_indices.insert (nn_indices.begin (), nn_indices.end ());
}
}
else
{
// Special case: When a feature must be computed at every point, there is no need for a neighborhood search
for (size_t idx = 0; idx < indices_->size (); ++idx)
spfh_indices.insert (static_cast<int> (idx));
}
// Initialize the arrays that will store the SPFH signatures
size_t data_size = spfh_indices.size ();
hist_f1.setZero (data_size, nr_bins_f1_);
hist_f2.setZero (data_size, nr_bins_f2_);
hist_f3.setZero (data_size, nr_bins_f3_);
// Compute SPFH signatures for every point that needs them
std::set<int>::iterator spfh_indices_itr = spfh_indices.begin ();
for (int i = 0; i < static_cast<int> (spfh_indices.size ()); ++i)
{
// Get the next point index
int p_idx = *spfh_indices_itr;
++spfh_indices_itr;
// Find the neighborhood around p_idx
if (this->searchForNeighbors (*surface_, p_idx, search_parameter_, nn_indices, nn_dists) == 0)
continue;
// Estimate the SPFH signature around p_idx
computePointSPFHSignature (*surface_, *normals_, p_idx, i, nn_indices, hist_f1, hist_f2, hist_f3);
// Populate a lookup table for converting a point index to its corresponding row in the spfh_hist_* matrices
spfh_hist_lookup[p_idx] = i;
}
}
template <typename PointInT, typename PointNT> void
pcl16::BoundaryEstimation<PointInT, PointNT, Eigen::MatrixXf>::computeFeatureEigen (pcl16::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 PointT> void
pcl::RadiusOutlierRemoval<PointT>::applyFilter (PointCloud &output)
{
if (search_radius_ == 0.0)
{
PCL_ERROR ("[pcl::%s::applyFilter] No radius defined!\n", getClassName ().c_str ());
output.width = output.height = 0;
output.points.clear ();
return;
}
// Initialize the spatial locator
if (!tree_)
{
if (input_->isOrganized ())
tree_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
else
tree_.reset (new pcl::search::KdTree<PointT> (false));
}
// Send the input dataset to the spatial locator
tree_->setInputCloud (input_);
// Allocate enough space to hold the results
std::vector<int> nn_indices (indices_->size ());
std::vector<float> nn_dists (indices_->size ());
output.points.resize (input_->points.size ()); // reserve enough space
removed_indices_->resize (input_->points.size ());
int nr_p = 0;
int nr_removed_p = 0;
// Go over all the points and check which doesn't have enough neighbors
for (size_t cp = 0; cp < indices_->size (); ++cp)
{
int k = tree_->radiusSearch ((*indices_)[cp], search_radius_, nn_indices, nn_dists);
// Check if the number of neighbors is larger than the user imposed limit
if (k < min_pts_radius_)
{
if (extract_removed_indices_)
{
(*removed_indices_)[nr_removed_p] = cp;
nr_removed_p++;
}
continue;
}
output.points[nr_p++] = input_->points[(*indices_)[cp]];
}
removed_indices_->resize (nr_removed_p);
output.points.resize (nr_p);
output.width = nr_p;
output.height = 1;
output.is_dense = true; // radiusSearch filters invalid points
}
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 PointNT, typename PointOutT> void
pcl::SHOTEstimationOMP<pcl::PointXYZRGBA, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
if (threads_ < 0)
threads_ = 1;
descLength_ = (b_describe_shape_) ? nr_grid_sector_ * (nr_shape_bins_ + 1) : 0;
descLength_ += (b_describe_color_) ? nr_grid_sector_ * (nr_color_bins_ + 1) : 0;
sqradius_ = search_radius_ * search_radius_;
radius3_4_ = (search_radius_ * 3) / 4;
radius1_4_ = search_radius_ / 4;
radius1_2_ = search_radius_ / 2;
if (output.points[0].descriptor.size () != (size_t)descLength_)
for (size_t idx = 0; idx < indices_->size (); ++idx)
output.points[idx].descriptor.resize (descLength_);
int data_size = indices_->size ();
Eigen::VectorXf *shot = new Eigen::VectorXf[threads_];
std::vector<std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > > rfs (threads_);
for (size_t i = 0; i < rfs.size (); ++i)
rfs[i].resize (3);
for (int i = 0; i < threads_; i++)
shot[i].setZero (descLength_);
// Iterating over the entire index vector
#pragma omp parallel for num_threads(threads_)
for (int idx = 0; idx < data_size; ++idx)
{
// 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_);
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists);
// Estimate the SHOT at each patch
#ifdef _OPENMP
int tid = omp_get_thread_num ();
#else
int tid = 0;
#endif
computePointSHOT ((*indices_)[idx], nn_indices, nn_dists, shot[tid], rfs[tid]);
// Copy into the resultant cloud
for (int d = 0; d < shot[tid].size (); ++d)
output.points[idx].descriptor[d] = shot[tid][d];
for (int d = 0; d < 9; ++d)
output.points[idx].rf[d] = rfs[tid][d / 3][d % 3];
}
delete[] shot;
}
template <typename PointInT, typename PointOutT> void
pcl::IntensitySpinEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
{
// Make sure a search radius is set
if (search_radius_ == 0.0)
{
ROS_ERROR ("[pcl::%s::computeFeature] The search radius must be set before computing the feature!",
getClassName ().c_str ());
output.width = output.height = 0;
output.points.clear ();
return;
}
// Make sure the spin image has valid dimensions
if (nr_intensity_bins_ <= 0)
{
ROS_ERROR ("[pcl::%s::computeFeature] The number of intensity bins must be greater than zero!",
getClassName ().c_str ());
output.width = output.height = 0;
output.points.clear ();
return;
}
if (nr_distance_bins_ <= 0)
{
ROS_ERROR ("[pcl::%s::computeFeature] The number of distance bins must be greater than zero!",
getClassName ().c_str ());
output.width = output.height = 0;
output.points.clear ();
return;
}
Eigen::MatrixXf intensity_spin_image (nr_intensity_bins_, nr_distance_bins_);
// Allocate enough space to hold the radiusSearch results
std::vector<int> nn_indices (surface_->points.size ());
std::vector<float> nn_dist_sqr (surface_->points.size ());
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
// Find neighbors within the search radius
// TODO: do we want to use searchForNeigbors instead?
int k = tree_->radiusSearch ((*indices_)[idx], search_radius_, nn_indices, nn_dist_sqr);
// Compute the intensity spin image
computeIntensitySpinImage (*surface_, search_radius_, sigma_, k, nn_indices, nn_dist_sqr, intensity_spin_image);
// Copy into the resultant cloud
for (int bin = 0; bin < intensity_spin_image.size (); ++bin)
output.points[idx].histogram[bin] = intensity_spin_image (bin);
}
}
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 PointT> void
pcl::RadiusOutlierRemoval<PointT>::applyFilterIndices (std::vector<int> &indices)
{
if (search_radius_ == 0.0)
{
PCL_ERROR ("[pcl::%s::applyFilter] No radius defined!\n", getClassName ().c_str ());
indices.clear ();
removed_indices_->clear ();
return;
}
// Initialize the search class
if (!searcher_)
{
if (input_->isOrganized ())
searcher_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
else
searcher_.reset (new pcl::search::KdTree<PointT> (false));
}
searcher_->setInputCloud (input_);
// The arrays to be used
std::vector<int> nn_indices (indices_->size ());
std::vector<float> nn_dists (indices_->size ());
indices.resize (indices_->size ());
removed_indices_->resize (indices_->size ());
int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator
for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii) // iii = input indices iterator
{
// Perform the radius search
// Note: k includes the query point, so is always at least 1
int k = searcher_->radiusSearch ((*indices_)[iii], search_radius_, nn_indices, nn_dists);
// Points having too few neighbors are outliers and are passed to removed indices
// Unless negative was set, then it's the opposite condition
if ((!negative_ && k <= min_pts_radius_) || (negative_ && k > min_pts_radius_))
{
if (extract_removed_indices_)
(*removed_indices_)[rii++] = (*indices_)[iii];
continue;
}
// Otherwise it was a normal point for output (inlier)
indices[oii++] = (*indices_)[iii];
}
// Resize the output arrays
indices.resize (oii);
removed_indices_->resize (rii);
}
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::PrincipalCurvaturesEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &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_);
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].principal_curvature[0] = output.points[idx].principal_curvature[1] = output.points[idx].principal_curvature[2] =
output.points[idx].pc1 = output.points[idx].pc2 = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
// Estimate the principal curvatures at each patch
computePointPrincipalCurvatures (*normals_, (*indices_)[idx], nn_indices,
output.points[idx].principal_curvature[0], output.points[idx].principal_curvature[1], output.points[idx].principal_curvature[2],
output.points[idx].pc1, output.points[idx].pc2);
}
}
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].principal_curvature[0] = output.points[idx].principal_curvature[1] = output.points[idx].principal_curvature[2] =
output.points[idx].pc1 = output.points[idx].pc2 = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
// Estimate the principal curvatures at each patch
computePointPrincipalCurvatures (*normals_, (*indices_)[idx], nn_indices,
output.points[idx].principal_curvature[0], output.points[idx].principal_curvature[1], output.points[idx].principal_curvature[2],
output.points[idx].pc1, output.points[idx].pc2);
}
}
}
template <typename PointT> void
pcl::getPointCloudDifference (
const pcl::PointCloud<PointT> &src,
const pcl::PointCloud<PointT> &,
double threshold, const boost::shared_ptr<pcl::search::Search<PointT> > &tree,
pcl::PointCloud<PointT> &output)
{
// We're interested in a single nearest neighbor only
std::vector<int> nn_indices (1);
std::vector<float> nn_distances (1);
// The src indices that do not have a neighbor in tgt
std::vector<int> src_indices;
// Iterate through the source data set
for (int i = 0; i < static_cast<int> (src.points.size ()); ++i)
{
if (!isFinite (src.points[i]))
continue;
// Search for the closest point in the target data set (number of neighbors to find = 1)
if (!tree->nearestKSearch (src.points[i], 1, nn_indices, nn_distances))
{
PCL_WARN ("No neighbor found for point %zu (%f %f %f)!\n", i, src.points[i].x, src.points[i].y, src.points[i].z);
continue;
}
if (nn_distances[0] > threshold)
src_indices.push_back (i);
}
// Allocate enough space and copy the basics
output.points.resize (src_indices.size ());
output.header = src.header;
output.width = static_cast<uint32_t> (src_indices.size ());
output.height = 1;
//if (src.is_dense)
output.is_dense = true;
//else
// It's not necessarily true that is_dense is false if cloud_in.is_dense is false
// To verify this, we would need to iterate over all points and check for NaNs
//output.is_dense = false;
// Copy all the data fields from the input cloud to the output one
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < src_indices.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (src.points[src_indices[i]], output.points[i]));
}
template <typename PointSource, typename PointTarget> double
pcl16::registration::TransformationValidationEuclidean<PointSource, PointTarget>::validateTransformation (
const PointCloudSourceConstPtr &cloud_src,
const PointCloudTargetConstPtr &cloud_tgt,
const Eigen::Matrix4f &transformation_matrix)
{
double fitness_score = 0.0;
// Transform the input dataset using the final transformation
pcl16::PointCloud<PointSource> input_transformed;
transformPointCloud (*cloud_src, input_transformed, transformation_matrix);
// Just in case
if (!tree_)
tree_.reset (new pcl16::KdTreeFLANN<PointTarget>);
tree_->setInputCloud (cloud_tgt);
std::vector<int> nn_indices (1);
std::vector<float> nn_dists (1);
// For each point in the source dataset
int nr = 0;
for (size_t i = 0; i < input_transformed.points.size (); ++i)
{
// Find its nearest neighbor in the target
tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists);
// Deal with occlusions (incomplete targets)
if (nn_dists[0] > max_range_)
continue;
// Optimization: use getVector4fMap instead, but make sure that the last coordinate is 0!
Eigen::Vector4f p1 (input_transformed.points[i].x,
input_transformed.points[i].y,
input_transformed.points[i].z, 0);
Eigen::Vector4f p2 (cloud_tgt->points[nn_indices[0]].x,
cloud_tgt->points[nn_indices[0]].y,
cloud_tgt->points[nn_indices[0]].z, 0);
// Calculate the fitness score
fitness_score += fabs ((p1-p2).squaredNorm ());
nr++;
}
if (nr > 0)
return (fitness_score / nr);
else
return (std::numeric_limits<double>::max ());
}
template <typename PointSource, typename PointTarget, typename Scalar> double
pcl::registration::TransformationValidationEuclidean<PointSource, PointTarget, Scalar>::validateTransformation (
const PointCloudSourceConstPtr &cloud_src,
const PointCloudTargetConstPtr &cloud_tgt,
const Matrix4 &transformation_matrix) const
{
double fitness_score = 0.0;
// Transform the input dataset using the final transformation
pcl::PointCloud<PointSource> input_transformed;
//transformPointCloud (*cloud_src, input_transformed, transformation_matrix);
input_transformed.resize (cloud_src->size ());
for (size_t i = 0; i < cloud_src->size (); ++i)
{
const PointSource &src = cloud_src->points[i];
PointTarget &tgt = input_transformed.points[i];
tgt.x = static_cast<float> (transformation_matrix (0, 0) * src.x + transformation_matrix (0, 1) * src.y + transformation_matrix (0, 2) * src.z + transformation_matrix (0, 3));
tgt.y = static_cast<float> (transformation_matrix (1, 0) * src.x + transformation_matrix (1, 1) * src.y + transformation_matrix (1, 2) * src.z + transformation_matrix (1, 3));
tgt.z = static_cast<float> (transformation_matrix (2, 0) * src.x + transformation_matrix (2, 1) * src.y + transformation_matrix (2, 2) * src.z + transformation_matrix (2, 3));
}
typename MyPointRepresentation::ConstPtr point_rep (new MyPointRepresentation);
tree_->setPointRepresentation (point_rep);
tree_->setInputCloud (cloud_tgt);
std::vector<int> nn_indices (1);
std::vector<float> nn_dists (1);
// For each point in the source dataset
int nr = 0;
for (size_t i = 0; i < input_transformed.points.size (); ++i)
{
// Find its nearest neighbor in the target
tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists);
// Deal with occlusions (incomplete targets)
if (nn_dists[0] > max_range_)
continue;
// Calculate the fitness score
fitness_score += nn_dists[0];
++nr;
}
if (nr > 0)
return (fitness_score / nr);
else
return (std::numeric_limits<double>::max ());
}
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> void
pcl16::NormalEstimationOMP<PointInT, Eigen::MatrixXf>::computeFeatureEigen (pcl16::PointCloud<Eigen::MatrixXf> &output)
{
float vpx, vpy, vpz;
getViewPoint (vpx, vpy, vpz);
output.is_dense = true;
// Resize the output dataset
output.points.resize (indices_->size (), 4);
// GCC 4.2.x seems to segfault with "internal compiler error" on MacOS X here
#if defined(_WIN32) || ((__GNUC__ > 4) && (__GNUC_MINOR__ > 2))
#pragma omp parallel for schedule (dynamic, threads_)
#endif
// Iterating over the entire index vector
for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
{
// 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_);
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;
}
// 16-bytes aligned placeholder for the XYZ centroid of a surface patch
Eigen::Vector4f xyz_centroid;
// Estimate the XYZ centroid
compute3DCentroid (*surface_, nn_indices, xyz_centroid);
// Placeholder for the 3x3 covariance matrix at each surface patch
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
// Compute the 3x3 covariance matrix
computeCovarianceMatrix (*surface_, nn_indices, xyz_centroid, covariance_matrix);
// Get the plane normal and surface curvature
solvePlaneParameters (covariance_matrix,
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 PointNT, typename PointOutT> void
pcl::IntensityGradientEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &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_);
output.is_dense = true;
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
PointOutT &p_out = output.points[idx];
if (!this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists))
{
p_out.gradient[0] = p_out.gradient[1] = p_out.gradient[2] = 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 /= (float)valid_neighbor_count;
Eigen::Vector3f normal = Eigen::Vector3f::Map (normals_->points[idx].normal);
Eigen::Vector3f gradient;
computePointIntensityGradient (*surface_, nn_indices, centroid.head<3> (), mean_intensity, normal, gradient);
p_out.gradient[0] = gradient[0];
p_out.gradient[1] = gradient[1];
p_out.gradient[2] = gradient[2];
}
}
template <typename PointInT, typename PointOutT> void
pcl::MomentInvariantsEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &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_);
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists);
computePointMomentInvariants (*surface_, nn_indices,
output.points[idx].j1, output.points[idx].j2, output.points[idx].j3);
}
}
double Recognizer::getFitnessScore(pcl::PointCloud<PointType>::Ptr target_cloud, pcl::PointCloud<PointType>::Ptr input_transformed) {
pcl::KdTreeFLANN<PointType>::Ptr tree(new pcl::KdTreeFLANN<PointType>);
double fitness_score = 0.0;
std::vector<int> nn_indices (1);
std::vector<float> nn_dists (1);
// For each point in the source dataset
// Initialize voxel grid filter object with the leaf size given by the user.
pcl::VoxelGrid<PointType> sor;
sor.setLeafSize(0.025, 0.025, 0.025);
pcl::PointCloud<PointType>::Ptr filteredTarget(new pcl::PointCloud<PointType>);
pcl::PointCloud<PointType>::Ptr filteredAligned(new pcl::PointCloud<PointType>);
sor.setInputCloud(target_cloud);
sor.filter(*filteredTarget);
sor.setInputCloud(input_transformed);
sor.filter(*filteredAligned);
tree->setInputCloud(filteredTarget);
int nr = 0;
for (size_t i = 0; i < filteredAligned->points.size(); ++i) {
Eigen::Vector4f p1 = Eigen::Vector4f(filteredAligned->points[i].x,
filteredAligned->points[i].y,
filteredAligned->points[i].z, 0);
if(isnan(p1(0)) || isnan(p1(1)) || isnan(p1(2)) || isinf(p1(0)) || isinf(p1(1)) || isinf(p1(2)))
continue;
// Find its nearest neighbor in the target
tree->nearestKSearch(filteredAligned->points[i], 1, nn_indices, nn_dists);
// Deal with occlusions (incomplete targets)
if (nn_dists[0] > std::numeric_limits<double>::max ())
continue;
Eigen::Vector4f p2 = Eigen::Vector4f(filteredTarget->points[nn_indices[0]].x,
filteredTarget->points[nn_indices[0]].y,
filteredTarget->points[nn_indices[0]].z, 0);
// Calculate the fitness score
fitness_score += fabs ((p1-p2).squaredNorm());
nr++;
}
if (nr > 0)
return(fitness_score/nr);
else
return(std::numeric_limits<double>::max());
}
template <typename PointSource, typename PointTarget, typename FeatureT> void
pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::findSimilarFeatures (
const FeatureCloud &input_features, const std::vector<int> &sample_indices,
std::vector<int> &corresponding_indices)
{
std::vector<int> nn_indices (k_correspondences_);
std::vector<float> nn_distances (k_correspondences_);
corresponding_indices.resize (sample_indices.size ());
for (size_t i = 0; i < sample_indices.size (); ++i)
{
// Find the k features nearest to input_features.points[sample_indices[i]]
feature_tree_->nearestKSearch (input_features, sample_indices[i], k_correspondences_, nn_indices, nn_distances);
// Select one at random and add it to corresponding_indices
int random_correspondence = getRandomIndex (k_correspondences_);
corresponding_indices[i] = nn_indices[random_correspondence];
}
}
template <typename PointInT, typename PointOutT> void
pcl16::NormalEstimationOMP<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
{
float vpx, vpy, vpz;
getViewPoint (vpx, vpy, vpz);
output.is_dense = true;
// Iterating over the entire index vector
#pragma omp parallel for schedule (dynamic, threads_)
for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
{
// 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_);
if (!isFinite ((*input_)[(*indices_)[idx]]) ||
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
// 16-bytes aligned placeholder for the XYZ centroid of a surface patch
Eigen::Vector4f xyz_centroid;
// Estimate the XYZ centroid
compute3DCentroid (*surface_, nn_indices, xyz_centroid);
// Placeholder for the 3x3 covariance matrix at each surface patch
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
// Compute the 3x3 covariance matrix
computeCovarianceMatrix (*surface_, nn_indices, xyz_centroid, covariance_matrix);
// Get the plane normal and surface curvature
solvePlaneParameters (covariance_matrix,
output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature);
flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx, vpy, vpz,
output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]);
}
}
template <typename PointSource, typename PointTarget> inline double
pcl::Registration<PointSource, PointTarget>::getFitnessScore (double max_range)
{
double fitness_score = 0.0;
// Transform the input dataset using the final transformation
PointCloudSource input_transformed;
transformPointCloud (*input_, input_transformed, final_transformation_);
std::vector<int> nn_indices (1);
std::vector<float> nn_dists (1);
// For each point in the source dataset
int nr = 0;
for (size_t i = 0; i < input_transformed.points.size (); ++i)
{
Eigen::Vector4f p1 = Eigen::Vector4f (input_transformed.points[i].x,
input_transformed.points[i].y,
input_transformed.points[i].z, 0);
// Find its nearest neighbor in the target
tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists);
// Deal with occlusions (incomplete targets)
if (nn_dists[0] > max_range)
continue;
Eigen::Vector4f p2 = Eigen::Vector4f (target_->points[nn_indices[0]].x,
target_->points[nn_indices[0]].y,
target_->points[nn_indices[0]].z, 0);
// Calculate the fitness score
fitness_score += fabs ((p1-p2).squaredNorm ());
nr++;
}
if (nr > 0)
return (fitness_score / nr);
else
return (std::numeric_limits<double>::max ());
}
template <typename PointT> void
pcl::getPointCloudDifference (
const pcl::PointCloud<PointT> &src,
double threshold,
const typename pcl::search::Search<PointT>::Ptr &tree,
pcl::PointCloud<PointT> &output)
{
// We're interested in a single nearest neighbor only
std::vector<int> nn_indices (1);
std::vector<float> nn_distances (1);
// The input cloud indices that do not have a neighbor in the target cloud
std::vector<int> src_indices;
// Iterate through the source data set
for (int i = 0; i < static_cast<int> (src.points.size ()); ++i)
{
// Ignore invalid points in the inpout cloud
if (!isFinite (src.points[i]))
continue;
// Search for the closest point in the target data set (number of neighbors to find = 1)
if (!tree->nearestKSearch (src.points[i], 1, nn_indices, nn_distances))
{
PCL_WARN ("No neighbor found for point %lu (%f %f %f)!\n", i, src.points[i].x, src.points[i].y, src.points[i].z);
continue;
}
// Add points without a corresponding point in the target cloud to the output cloud
if (nn_distances[0] > threshold)
src_indices.push_back (i);
}
// Copy all the data fields from the input cloud to the output one
copyPointCloud (src, src_indices, output);
// Output is always dense, as invalid points in the input cloud are ignored
output.is_dense = true;
}
template <typename PointNT> void
pcl::MarchingCubesHoppe<PointNT>::voxelizeData ()
{
const bool is_far_ignored = dist_ignore_ > 0.0f;
for (int x = 0; x < res_x_; ++x)
{
const int y_start = x * res_y_ * res_z_;
for (int y = 0; y < res_y_; ++y)
{
const int z_start = y_start + y * res_z_;
for (int z = 0; z < res_z_; ++z)
{
std::vector<int> nn_indices (1, 0);
std::vector<float> nn_sqr_dists (1, 0.0f);
const Eigen::Vector3f point = (lower_boundary_ + size_voxel_ * Eigen::Array3f (x, y, z)).matrix ();
PointNT p;
p.getVector3fMap () = point;
tree_->nearestKSearch (p, 1, nn_indices, nn_sqr_dists);
if (!is_far_ignored || nn_sqr_dists[0] < dist_ignore_)
{
const Eigen::Vector3f normal = input_->points[nn_indices[0]].getNormalVector3fMap ();
if (!std::isnan (normal (0)) && normal.norm () > 0.5f)
grid_[z_start + z] = normal.dot (
point - input_->points[nn_indices[0]].getVector3fMap ());
}
}
}
}
}
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::PrincipalCurvaturesEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
// Check if input was set
if (!normals_)
{
ROS_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!", getClassName ().c_str ());
output.width = output.height = 0;
output.points.clear ();
return;
}
if (normals_->points.size () != surface_->points.size ())
{
ROS_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset (%zu) differs from the number of points in the dataset containing the normals (%zu)!",
getClassName ().c_str (), surface_->points.size (), normals_->points.size ());
output.width = output.height = 0;
output.points.clear ();
return;
}
// 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_);
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists);
// Estimate the principal curvatures at each patch
computePointPrincipalCurvatures (*normals_, (*indices_)[idx], nn_indices,
output.points[idx].principal_curvature[0], output.points[idx].principal_curvature[1], output.points[idx].principal_curvature[2],
output.points[idx].pc1, output.points[idx].pc2);
}
}
