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); }