template <typename PointInT, typename PointOutT> void pcl::MovingLeastSquares<PointInT, PointOutT>::performProcessing (PointCloudOut &output) { // Compute the number of coefficients nr_coeff_ = (order_ + 1) * (order_ + 2) / 2; // Allocate enough space to hold the results of nearest neighbor searches // \note resize is irrelevant for a radiusSearch (). std::vector<int> nn_indices; std::vector<float> nn_sqr_dists; // For all points for (size_t cp = 0; cp < indices_->size (); ++cp) { // Get the initial estimates of point positions and their neighborhoods if (!searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists)) continue; // Check the number of nearest neighbors for normal estimation (and later // for polynomial fit as well) if (nn_indices.size () < 3) continue; PointCloudOut projected_points; NormalCloud projected_points_normals; // Get a plane approximating the local surface's tangent and project point onto it int index = (*indices_)[cp]; computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points, projected_points_normals, *corresponding_input_indices_, mls_results_[index]); // Copy all information from the input cloud to the output points (not doing any interpolation) for (size_t pp = 0; pp < projected_points.size (); ++pp) copyMissingFields (input_->points[(*indices_)[cp]], projected_points[pp]); // Append projected points to output output.insert (output.end (), projected_points.begin (), projected_points.end ()); if (compute_normals_) normals_->insert (normals_->end (), projected_points_normals.begin (), projected_points_normals.end ()); } // Perform the distinct-cloud or voxel-grid upsampling performUpsampling (output); }
template <typename PointInT, typename PointOutT> void pcl::MovingLeastSquares<PointInT, PointOutT>::performProcessing (PointCloudOut &output) { // Compute the number of coefficients nr_coeff_ = (order_ + 1) * (order_ + 2) / 2; // Allocate enough space to hold the results of nearest neighbor searches // \note resize is irrelevant for a radiusSearch (). std::vector<int> nn_indices; std::vector<float> nn_sqr_dists; // For all points for (size_t cp = 0; cp < indices_->size (); ++cp) { // Get the initial estimates of point positions and their neighborhoods if (!searchForNeighbors (int (cp), nn_indices, nn_sqr_dists)) continue; // Check the number of nearest neighbors for normal estimation (and later // for polynomial fit as well) if (nn_indices.size () < 3) continue; PointCloudOut projected_points; NormalCloud projected_points_normals; // Get a plane approximating the local surface's tangent and project point onto it computeMLSPointNormal (int (cp), *input_, nn_indices, nn_sqr_dists, projected_points, projected_points_normals); // Append projected points to output output.insert (output.end (), projected_points.begin (), projected_points.end ()); if (compute_normals_) normals_->insert (normals_->end (), projected_points_normals.begin (), projected_points_normals.end ()); } // For the voxel grid upsampling method, generate the voxel grid and dilate it // Then, project the newly obtained points to the MLS surface if (upsample_method_ == VOXEL_GRID_DILATION) { MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_); for (int iteration = 0; iteration < dilation_iteration_num_; ++iteration) voxel_grid.dilate (); BOOST_FOREACH (typename MLSVoxelGrid::HashMap::value_type voxel, voxel_grid.voxel_grid_) { // Get 3D position of point Eigen::Vector3f pos; voxel_grid.getPosition (voxel.first, pos); PointInT p; p.x = pos[0]; p.y = pos[1]; p.z = pos[2]; std::vector<int> nn_indices; std::vector<float> nn_dists; tree_->nearestKSearch (p, 1, nn_indices, nn_dists); int input_index = nn_indices.front (); // If the closest point did not have a valid MLS fitting result // OR if it is too far away from the sampled point if (mls_results_[input_index].valid == false) continue; Eigen::Vector3f add_point = p.getVector3fMap (), input_point = input_->points[input_index].getVector3fMap (); Eigen::Vector3d aux = mls_results_[input_index].u; Eigen::Vector3f u = aux.cast<float> (); aux = mls_results_[input_index].v; Eigen::Vector3f v = aux.cast<float> (); float u_disp = (add_point - input_point).dot (u), v_disp = (add_point - input_point).dot (v); PointOutT result_point; pcl::Normal result_normal; projectPointToMLSSurface (u_disp, v_disp, mls_results_[input_index].u, mls_results_[input_index].v, mls_results_[input_index].plane_normal, mls_results_[input_index].curvature, input_point, mls_results_[input_index].c_vec, mls_results_[input_index].num_neighbors, result_point, result_normal); float d_before = (pos - input_point).norm (), d_after = (result_point.getVector3fMap () - input_point). norm(); if (d_after > d_before) continue; output.push_back (result_point); if (compute_normals_) normals_->push_back (result_normal); } }
template <typename PointInT, typename PointOutT> void pcl::MovingLeastSquaresOMP<PointInT, PointOutT>::performProcessing (PointCloudOut &output) { // Compute the number of coefficients nr_coeff_ = (order_ + 1) * (order_ + 2) / 2; // (Maximum) number of threads unsigned int threads = threads_ == 0 ? 1 : threads_; // Create temporaries for each thread in order to avoid synchronization typename PointCloudOut::CloudVectorType projected_points (threads); typename NormalCloud::CloudVectorType projected_points_normals (threads); std::vector<PointIndices> corresponding_input_indices (threads); // For all points #pragma omp parallel for schedule (dynamic,1000) num_threads (threads) for (int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp) { // Allocate enough space to hold the results of nearest neighbor searches // \note resize is irrelevant for a radiusSearch (). std::vector<int> nn_indices; std::vector<float> nn_sqr_dists; // Get the initial estimates of point positions and their neighborhoods if (this->searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists)) { // Check the number of nearest neighbors for normal estimation (and later // for polynomial fit as well) if (nn_indices.size () >= 3) { // This thread's ID (range 0 to threads-1) int tn = omp_get_thread_num (); // Size of projected points before computeMLSPointNormal () adds points size_t pp_size = projected_points[tn].size (); // Get a plane approximating the local surface's tangent and project point onto it int index = (*indices_)[cp]; this->computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points[tn], projected_points_normals[tn], corresponding_input_indices[tn], this->mls_results_[index]); // Copy all information from the input cloud to the output points (not doing any interpolation) for (size_t pp = pp_size; pp < projected_points[tn].size (); ++pp) this->copyMissingFields (input_->points[(*indices_)[cp]], projected_points[tn][pp]); } } } // Combine all threads' results into the output vectors for (unsigned int tn = 0; tn < threads; ++tn) { output.insert (output.end (), projected_points[tn].begin (), projected_points[tn].end ()); corresponding_input_indices_->indices.insert (corresponding_input_indices_->indices.end (), corresponding_input_indices[tn].indices.begin (), corresponding_input_indices[tn].indices.end ()); if (compute_normals_) normals_->insert (normals_->end (), projected_points_normals[tn].begin (), projected_points_normals[tn].end ()); } // Perform the distinct-cloud or voxel-grid upsampling this->performUpsampling (output); }
template <typename PointInT, typename PointOutT> void pcl::MovingLeastSquares<PointInT, PointOutT>::performProcessing (PointCloudOut &output) { // Compute the number of coefficients nr_coeff_ = (order_ + 1) * (order_ + 2) / 2; // Allocate enough space to hold the results of nearest neighbor searches // \note resize is irrelevant for a radiusSearch (). std::vector<int> nn_indices; std::vector<float> nn_sqr_dists; typedef typename pcl::traits::fieldList<typename PointCloudIn::PointType>::type FieldListInput; typedef typename pcl::traits::fieldList<typename PointCloudOut::PointType>::type FieldListOutput; // For all points for (size_t cp = 0; cp < indices_->size (); ++cp) { // Get the initial estimates of point positions and their neighborhoods if (!searchForNeighbors (int (cp), nn_indices, nn_sqr_dists)) continue; // Check the number of nearest neighbors for normal estimation (and later // for polynomial fit as well) if (nn_indices.size () < 3) continue; PointCloudOut projected_points; NormalCloud projected_points_normals; // Get a plane approximating the local surface's tangent and project point onto it computeMLSPointNormal (int (cp), *input_, nn_indices, nn_sqr_dists, projected_points, projected_points_normals); /// Copy RGB information if available float rgb_input; bool rgb_exists_input; pcl::for_each_type<FieldListInput> (pcl::CopyIfFieldExists<typename PointCloudIn::PointType, float> ( input_->points[(*indices_)[cp]], "rgb", rgb_exists_input, rgb_input)); if (rgb_exists_input) { for (size_t pp = 0; pp < projected_points.size (); ++pp) pcl::for_each_type<FieldListOutput> (pcl::SetIfFieldExists<typename PointCloudOut::PointType, float> ( projected_points.points[pp], "rgb", rgb_input)); } // Append projected points to output output.insert (output.end (), projected_points.begin (), projected_points.end ()); if (compute_normals_) normals_->insert (normals_->end (), projected_points_normals.begin (), projected_points_normals.end ()); } if (upsample_method_ == DISTINCT_CLOUD) { for (size_t dp_i = 0; dp_i < distinct_cloud_->size (); ++dp_i) // dp_i = distinct_point_i { // Distinct cloud may have nan points, skip them if (!pcl_isfinite (distinct_cloud_->points[dp_i].x)) continue; // Get 3D position of point //Eigen::Vector3f pos = distinct_cloud_->points[dp_i].getVector3fMap (); std::vector<int> nn_indices; std::vector<float> nn_dists; tree_->nearestKSearch (distinct_cloud_->points[dp_i], 1, nn_indices, nn_dists); int input_index = nn_indices.front (); // If the closest point did not have a valid MLS fitting result // OR if it is too far away from the sampled point if (mls_results_[input_index].valid == false) continue; Eigen::Vector3f add_point = distinct_cloud_->points[dp_i].getVector3fMap (), input_point = input_->points[input_index].getVector3fMap (); Eigen::Vector3d aux = mls_results_[input_index].u; Eigen::Vector3f u = aux.cast<float> (); aux = mls_results_[input_index].v; Eigen::Vector3f v = aux.cast<float> (); float u_disp = (add_point - input_point).dot (u), v_disp = (add_point - input_point).dot (v); PointOutT result_point; pcl::Normal result_normal; projectPointToMLSSurface (u_disp, v_disp, mls_results_[input_index].u, mls_results_[input_index].v, mls_results_[input_index].plane_normal, mls_results_[input_index].curvature, input_point, mls_results_[input_index].c_vec, mls_results_[input_index].num_neighbors, result_point, result_normal); /// Copy RGB information if available float rgb_input; bool rgb_exists_input; pcl::for_each_type<FieldListInput> (pcl::CopyIfFieldExists<typename PointCloudIn::PointType, float> ( input_->points[input_index], "rgb", rgb_exists_input, rgb_input)); if (rgb_exists_input) { pcl::for_each_type<FieldListOutput> (pcl::SetIfFieldExists<typename PointCloudOut::PointType, float> ( result_point, "rgb", rgb_input)); } output.push_back (result_point); if (compute_normals_) normals_->push_back (result_normal); } } // For the voxel grid upsampling method, generate the voxel grid and dilate it // Then, project the newly obtained points to the MLS surface if (upsample_method_ == VOXEL_GRID_DILATION) { MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_); for (int iteration = 0; iteration < dilation_iteration_num_; ++iteration) voxel_grid.dilate (); for (typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid.voxel_grid_.begin (); m_it != voxel_grid.voxel_grid_.end (); ++m_it) { // Get 3D position of point Eigen::Vector3f pos; voxel_grid.getPosition (m_it->first, pos); PointInT p; p.x = pos[0]; p.y = pos[1]; p.z = pos[2]; std::vector<int> nn_indices; std::vector<float> nn_dists; tree_->nearestKSearch (p, 1, nn_indices, nn_dists); int input_index = nn_indices.front (); // If the closest point did not have a valid MLS fitting result // OR if it is too far away from the sampled point if (mls_results_[input_index].valid == false) continue; Eigen::Vector3f add_point = p.getVector3fMap (), input_point = input_->points[input_index].getVector3fMap (); Eigen::Vector3d aux = mls_results_[input_index].u; Eigen::Vector3f u = aux.cast<float> (); aux = mls_results_[input_index].v; Eigen::Vector3f v = aux.cast<float> (); float u_disp = (add_point - input_point).dot (u), v_disp = (add_point - input_point).dot (v); PointOutT result_point; pcl::Normal result_normal; projectPointToMLSSurface (u_disp, v_disp, mls_results_[input_index].u, mls_results_[input_index].v, mls_results_[input_index].plane_normal, mls_results_[input_index].curvature, input_point, mls_results_[input_index].c_vec, mls_results_[input_index].num_neighbors, result_point, result_normal); float d_before = (pos - input_point).norm (), d_after = (result_point.getVector3fMap () - input_point). norm(); if (d_after > d_before) continue; /// Copy RGB information if available float rgb_input; bool rgb_exists_input; pcl::for_each_type<FieldListInput> (pcl::CopyIfFieldExists<typename PointCloudIn::PointType, float> ( input_->points[input_index], "rgb", rgb_exists_input, rgb_input)); if (rgb_exists_input) { pcl::for_each_type<FieldListOutput> (pcl::SetIfFieldExists<typename PointCloudOut::PointType, float> ( result_point, "rgb", rgb_input)); } output.push_back (result_point); if (compute_normals_) normals_->push_back (result_normal); } } }