template <typename PointT> void pcl::StatisticalOutlierRemoval<PointT>::applyFilterIndices (std::vector<int> &indices) { // 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 (mean_k_); std::vector<float> nn_dists (mean_k_); std::vector<float> distances (indices_->size ()); indices.resize (indices_->size ()); removed_indices_->resize (indices_->size ()); int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator // First pass: Compute the mean distances for all points with respect to their k nearest neighbors for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii) // iii = input indices iterator { if (!pcl_isfinite (input_->points[(*indices_)[iii]].x) || !pcl_isfinite (input_->points[(*indices_)[iii]].y) || !pcl_isfinite (input_->points[(*indices_)[iii]].z)) { distances[iii] = 0.0; continue; } // Perform the nearest k search if (searcher_->nearestKSearch ((*indices_)[iii], mean_k_ + 1, nn_indices, nn_dists) == 0) { distances[iii] = 0.0; PCL_WARN ("[pcl::%s::applyFilter] Searching for the closest %d neighbors failed.\n", getClassName ().c_str (), mean_k_); continue; } // Calculate the mean distance to its neighbors double dist_sum = 0.0; for (int k = 1; k < mean_k_ + 1; ++k) // k = 0 is the query point dist_sum += sqrt (nn_dists[k]); distances[iii] = static_cast<float> (dist_sum / mean_k_); } // Estimate the mean and the standard deviation of the distance vector double mean, stddev; getMeanStd (distances, mean, stddev); double distance_threshold = mean + std_mul_ * stddev; // Second pass: Classify the points on the computed distance threshold for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii) // iii = input indices iterator { // Points having a too high average distance are outliers and are passed to removed indices // Unless negative was set, then it's the opposite condition if ((!negative_ && distances[iii] > distance_threshold) || (negative_ && distances[iii] <= distance_threshold)) { 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); }
void pcl::StatisticalOutlierRemoval<sensor_msgs::PointCloud2>::applyFilter (PointCloud2 &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) { ROS_ERROR ("[pcl::%s::applyFilter] Input dataset doesn't have x-y-z coordinates!", getClassName ().c_str ()); output.width = output.height = 0; output.data.clear (); return; } if (std_mul_ == 0.0) { ROS_ERROR ("[pcl::%s::applyFilter] Standard deviation multipler not set!", getClassName ().c_str ()); output.width = output.height = 0; output.data.clear (); return; } // Initialize the spatial locator //initTree (spatial_locator_type_, tree_, k_); // TODO: fix this tree_.reset (new pcl::KdTreeFLANN<pcl::PointXYZ>); // Send the input dataset to the spatial locator pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg (*input_, *cloud); 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 ()); // 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; ROS_WARN ("[pcl::%s::applyFilter] Searching for the closest %d neighbors failed.", 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] = dist_sum / (mean_k_-1); } // Estimate the mean and the standard deviation of the distance vector double mean, stddev; 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 // Build a new cloud by neglecting outliers int nr_p = 0; for (size_t cp = 0; cp < indices_->size (); ++cp) { if (negative_) { if (distances[cp] <= distance_threshold) continue; } else { if (distances[cp] > distance_threshold) 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; }
template <typename PointT> void pcl::StatisticalOutlierRemoval<PointT>::applyFilter (PointCloud &output) { if (std_mul_ == 0.0) { PCL_ERROR ("[pcl::%s::applyFilter] Standard deviation multiplier not set!\n", getClassName ().c_str ()); output.width = output.height = 0; output.points.clear (); return; } if (input_->points.empty ()) { 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 (mean_k_); std::vector<float> nn_dists (mean_k_); std::vector<float> distances (indices_->size ()); // Go over all the points and calculate the mean or smallest distance for (size_t cp = 0; cp < indices_->size (); ++cp) { if (!pcl_isfinite (input_->points[(*indices_)[cp]].x) || !pcl_isfinite (input_->points[(*indices_)[cp]].y) || !pcl_isfinite (input_->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)); } // Estimate the mean and the standard deviation of the distance vector double mean, stddev; getMeanStd (distances, mean, stddev); double distance_threshold = mean + std_mul_ * stddev; // a distance that is bigger than this signals an outlier output.points.resize (input_->points.size ()); // reserve enough space removed_indices_->resize (input_->points.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; } } output.points[nr_p++] = input_->points[(*indices_)[cp]]; } output.points.resize (nr_p); output.width = nr_p; output.height = 1; output.is_dense = true; // nearestKSearch filters invalid points removed_indices_->resize (nr_removed_p); }