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