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");
}
}
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 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]);
}
}
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> 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 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 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> 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, 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 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 PointT> void
pcl::getApproximateIndices (
const typename pcl::PointCloud<PointT>::ConstPtr &cloud_in,
const typename pcl::PointCloud<PointT>::ConstPtr &cloud_ref,
std::vector<int> &indices)
{
pcl::KdTreeFLANN<PointT> tree;
tree.setInputCloud (cloud_ref);
std::vector<int> nn_idx (1);
std::vector<float> nn_dists (1);
indices.resize (cloud_in->points.size ());
for (size_t i = 0; i < cloud_in->points.size (); ++i)
{
tree.nearestKSearch (*cloud_in, i, 1, nn_idx, nn_dists);
indices[i] = nn_idx[0];
}
}
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 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);
}
}
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
// 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;
std::vector<int> spfh_hist_lookup (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];
this->searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists);
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 (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 (size_t i = 0; i < spfh_indices.size (); ++i)
{
// Get the next point index
int p_idx = *spfh_indices_itr;
++spfh_indices_itr;
// Find the neighborhood around p_idx
this->searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists);
// 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;
}
// Intialize the array that will store the FPFH signature
int nr_bins = nr_bins_f1_ + nr_bins_f2_ + nr_bins_f3_;
fpfh_histogram_.setZero (nr_bins);
// Iterate over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
// Find the indices of point idx's neighbors...
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists);
// ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices
// instead of indices into surface_->points
for (size_t i = 0; i < nn_indices.size (); ++i)
nn_indices[i] = spfh_hist_lookup[nn_indices[i]];
// Compute the FPFH signature (i.e. compute a weighted combination of local SPFH signatures) ...
weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_);
// ...and copy it into the output 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> void
pcl::FPFHEstimation<PointInT, PointNT, Eigen::MatrixXf>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
{
// Set up the output channels
output.channels["fpfh"].name = "fpfh";
output.channels["fpfh"].offset = 0;
output.channels["fpfh"].size = 4;
output.channels["fpfh"].count = 33;
output.channels["fpfh"].datatype = sensor_msgs::PointField::FLOAT32;
// 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::vector<int> spfh_hist_lookup;
this->computeSPFHSignatures (spfh_hist_lookup, hist_f1_, hist_f2_, hist_f3_);
// Intialize the array that will store the FPFH signature
output.points.resize (indices_->size (), nr_bins_f1_ + nr_bins_f2_ + nr_bins_f3_);
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)
{
// Iterate 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;
}
// ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices
// instead of indices into surface_->points
for (size_t i = 0; i < nn_indices.size (); ++i)
nn_indices[i] = spfh_hist_lookup[nn_indices[i]];
// Compute the FPFH signature (i.e. compute a weighted combination of local SPFH signatures) ...
this->weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_);
output.points.row (idx) = fpfh_histogram_;
}
}
else
{
// Iterate 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;
}
// ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices
// instead of indices into surface_->points
for (size_t i = 0; i < nn_indices.size (); ++i)
nn_indices[i] = spfh_hist_lookup[nn_indices[i]];
// Compute the FPFH signature (i.e. compute a weighted combination of local SPFH signatures) ...
this->weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_);
output.points.row (idx) = fpfh_histogram_;
}
}
}
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
// 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::vector<int> spfh_hist_lookup;
computeSPFHSignatures (spfh_hist_lookup, hist_f1_, hist_f2_, hist_f3_);
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)
{
// Iterate 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)
{
for (int d = 0; d < fpfh_histogram_.size (); ++d)
output.points[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
// ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices
// instead of indices into surface_->points
for (size_t i = 0; i < nn_indices.size (); ++i)
nn_indices[i] = spfh_hist_lookup[nn_indices[i]];
// Compute the FPFH signature (i.e. compute a weighted combination of local SPFH signatures) ...
weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_);
// ...and copy it into the output cloud
for (int d = 0; d < fpfh_histogram_.size (); ++d)
output.points[idx].histogram[d] = fpfh_histogram_[d];
}
}
else
{
// Iterate 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)
{
for (int d = 0; d < fpfh_histogram_.size (); ++d)
output.points[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
// ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices
// instead of indices into surface_->points
for (size_t i = 0; i < nn_indices.size (); ++i)
nn_indices[i] = spfh_hist_lookup[nn_indices[i]];
// Compute the FPFH signature (i.e. compute a weighted combination of local SPFH signatures) ...
weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_);
// ...and copy it into the output cloud
for (int d = 0; d < fpfh_histogram_.size (); ++d)
output.points[idx].histogram[d] = fpfh_histogram_[d];
}
}
}
void
pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
{
output.is_dense = true;
// If fields x/y/z are not present, we cannot filter
if (x_idx_ == -1 || y_idx_ == -1 || z_idx_ == -1)
{
PCL_ERROR ("[pcl::%s::applyFilter] Input dataset doesn't have x-y-z coordinates!\n", getClassName ().c_str ());
output.width = output.height = 0;
output.data.clear ();
return;
}
if (std_mul_ == 0.0)
{
PCL_ERROR ("[pcl::%s::applyFilter] Standard deviation multipler not set!\n", getClassName ().c_str ());
output.width = output.height = 0;
output.data.clear ();
return;
}
// Send the input dataset to the spatial locator
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2 (*input_, *cloud);
// Initialize the spatial locator
if (!tree_)
{
if (cloud->isOrganized ())
tree_.reset (new pcl::search::OrganizedNeighbor<pcl::PointXYZ> ());
else
tree_.reset (new pcl::search::KdTree<pcl::PointXYZ> (false));
}
tree_->setInputCloud (cloud);
// Allocate enough space to hold the results
std::vector<int> nn_indices (mean_k_);
std::vector<float> nn_dists (mean_k_);
std::vector<float> distances (indices_->size ());
int valid_distances = 0;
// Go over all the points and calculate the mean or smallest distance
for (size_t cp = 0; cp < indices_->size (); ++cp)
{
if (!pcl_isfinite (cloud->points[(*indices_)[cp]].x) ||
!pcl_isfinite (cloud->points[(*indices_)[cp]].y) ||
!pcl_isfinite (cloud->points[(*indices_)[cp]].z))
{
distances[cp] = 0;
continue;
}
if (tree_->nearestKSearch ((*indices_)[cp], mean_k_, nn_indices, nn_dists) == 0)
{
distances[cp] = 0;
PCL_WARN ("[pcl::%s::applyFilter] Searching for the closest %d neighbors failed.\n", getClassName ().c_str (), mean_k_);
continue;
}
// Minimum distance (if mean_k_ == 2) or mean distance
double dist_sum = 0;
for (int j = 1; j < mean_k_; ++j)
dist_sum += sqrt (nn_dists[j]);
distances[cp] = static_cast<float> (dist_sum / (mean_k_ - 1));
valid_distances++;
}
// Estimate the mean and the standard deviation of the distance vector
double sum = 0, sq_sum = 0;
for (size_t i = 0; i < distances.size (); ++i)
{
sum += distances[i];
sq_sum += distances[i] * distances[i];
}
double mean = sum / static_cast<double>(valid_distances);
double variance = (sq_sum - sum * sum / static_cast<double>(valid_distances)) / (static_cast<double>(valid_distances) - 1);
double stddev = sqrt (variance);
//getMeanStd (distances, mean, stddev);
double distance_threshold = mean + std_mul_ * stddev; // a distance that is bigger than this signals an outlier
// Copy the common fields
output.is_bigendian = input_->is_bigendian;
output.point_step = input_->point_step;
output.height = 1;
output.data.resize (indices_->size () * input_->point_step); // reserve enough space
removed_indices_->resize (input_->data.size ());
// Build a new cloud by neglecting outliers
int nr_p = 0;
int nr_removed_p = 0;
for (int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp)
{
if (negative_)
{
if (distances[cp] <= distance_threshold)
{
if (extract_removed_indices_)
{
(*removed_indices_)[nr_removed_p] = cp;
nr_removed_p++;
}
continue;
}
}
else
{
if (distances[cp] > distance_threshold)
{
if (extract_removed_indices_)
{
(*removed_indices_)[nr_removed_p] = cp;
nr_removed_p++;
}
continue;
}
}
memcpy (&output.data[nr_p * output.point_step], &input_->data[(*indices_)[cp] * output.point_step],
output.point_step);
nr_p++;
}
output.width = nr_p;
output.data.resize (output.width * output.point_step);
output.row_step = output.point_step * output.width;
removed_indices_->resize (nr_removed_p);
}
