template <typename PointT, typename NormalT> void pcl::RegionGrowingHSV<PointT, NormalT>::extract(std::vector <pcl::PointIndices>& clusters) { clusters_.clear(); clusters.clear(); point_neighbours_.clear(); point_labels_.clear(); num_pts_in_segment_.clear(); point_distances_.clear(); segment_neighbours_.clear(); segment_distances_.clear(); segment_labels_.clear(); number_of_segments_ = 0; bool segmentation_is_possible = initCompute(); if (!segmentation_is_possible) { deinitCompute(); return; } segmentation_is_possible = prepareForSegmentation(); if (!segmentation_is_possible) { deinitCompute(); return; } std::cout<< "Start find point neighbours"<<std::endl; findPointNeighbours(); std::cout<< "Start region growing"<<std::endl; applySmoothRegionGrowingAlgorithm(); std::cout<< "assemble regions"<<std::endl; RegionGrowing<PointT, NormalT>::assembleRegions(); std::cout<< "Start find region neighbours"<<std::endl; findSegmentNeighbours(); std::cout<< "Start merging region"<<std::endl; applyRegionMergingAlgorithm(); std::vector<pcl::PointIndices>::iterator cluster_iter = clusters_.begin(); std::cout << "Num of clusters is: " << clusters_.size() <<std::endl; while (cluster_iter != clusters_.end()) { if (cluster_iter->indices.size() < min_pts_per_cluster_ || cluster_iter->indices.size() > max_pts_per_cluster_) { cluster_iter = clusters_.erase(cluster_iter); } else cluster_iter++; } clusters.reserve(clusters_.size()); std::copy(clusters_.begin(), clusters_.end(), std::back_inserter(clusters)); findSegmentNeighbours(); deinitCompute(); }
template <typename PointT> void pcl16::SACSegmentation<PointT>::segment (PointIndices &inliers, ModelCoefficients &model_coefficients) { // Copy the header information inliers.header = model_coefficients.header = input_->header; if (!initCompute ()) { inliers.indices.clear (); model_coefficients.values.clear (); return; } // Initialize the Sample Consensus model and set its parameters if (!initSACModel (model_type_)) { PCL16_ERROR ("[pcl16::%s::segment] Error initializing the SAC model!\n", getClassName ().c_str ()); deinitCompute (); inliers.indices.clear (); model_coefficients.values.clear (); return; } // Initialize the Sample Consensus method and set its parameters initSAC (method_type_); if (!sac_->computeModel (0)) { PCL16_ERROR ("[pcl16::%s::segment] Error segmenting the model! No solution found.\n", getClassName ().c_str ()); deinitCompute (); inliers.indices.clear (); model_coefficients.values.clear (); return; } // Get the model inliers sac_->getInliers (inliers.indices); // Get the model coefficients Eigen::VectorXf coeff; sac_->getModelCoefficients (coeff); // If the user needs optimized coefficients if (optimize_coefficients_) { Eigen::VectorXf coeff_refined; model_->optimizeModelCoefficients (inliers.indices, coeff, coeff_refined); model_coefficients.values.resize (coeff_refined.size ()); memcpy (&model_coefficients.values[0], &coeff_refined[0], coeff_refined.size () * sizeof (float)); // Refine inliers model_->selectWithinDistance (coeff_refined, threshold_, inliers.indices); } else { model_coefficients.values.resize (coeff.size ()); memcpy (&model_coefficients.values[0], &coeff[0], coeff.size () * sizeof (float)); } deinitCompute (); }
template <typename PointT> void pcl::SupervoxelClustering<PointT>::extract (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters) { //timer_.reset (); //double t_start = timer_.getTime (); //std::cout << "Init compute \n"; bool segmentation_is_possible = initCompute (); if ( !segmentation_is_possible ) { deinitCompute (); return; } //std::cout << "Preparing for segmentation \n"; segmentation_is_possible = prepareForSegmentation (); if ( !segmentation_is_possible ) { deinitCompute (); return; } //double t_prep = timer_.getTime (); //std::cout << "Placing Seeds" << std::endl; std::vector<int> seed_indices; selectInitialSupervoxelSeeds (seed_indices); //std::cout << "Creating helpers "<<std::endl; createSupervoxelHelpers (seed_indices); //double t_seeds = timer_.getTime (); //std::cout << "Expanding the supervoxels" << std::endl; int max_depth = static_cast<int> (1.8f*seed_resolution_/resolution_); expandSupervoxels (max_depth); //double t_iterate = timer_.getTime (); //std::cout << "Making Supervoxel structures" << std::endl; makeSupervoxels (supervoxel_clusters); //double t_supervoxels = timer_.getTime (); // std::cout << "--------------------------------- Timing Report --------------------------------- \n"; // std::cout << "Time to prep (normals, neighbors, voxelization)="<<t_prep-t_start<<" ms\n"; // std::cout << "Time to seed clusters ="<<t_seeds-t_prep<<" ms\n"; // std::cout << "Time to expand clusters ="<<t_iterate-t_seeds<<" ms\n"; // std::cout << "Time to create supervoxel structures ="<<t_supervoxels-t_iterate<<" ms\n"; // std::cout << "Total run time ="<<t_supervoxels-t_start<<" ms\n"; // std::cout << "--------------------------------------------------------------------------------- \n"; deinitCompute (); }
template <typename PointT> void pcl16::EuclideanClusterExtraction<PointT>::extract (std::vector<PointIndices> &clusters) { if (!initCompute () || (input_ != 0 && input_->points.empty ()) || (indices_ != 0 && indices_->empty ())) { clusters.clear (); return; } // Initialize the spatial locator if (!tree_) { if (input_->isOrganized ()) tree_.reset (new pcl16::search::OrganizedNeighbor<PointT> ()); else tree_.reset (new pcl16::search::KdTree<PointT> (false)); } // Send the input dataset to the spatial locator tree_->setInputCloud (input_, indices_); extractEuclideanClusters (*input_, *indices_, tree_, static_cast<float> (cluster_tolerance_), clusters, min_pts_per_cluster_, max_pts_per_cluster_); //tree_->setInputCloud (input_); //extractEuclideanClusters (*input_, tree_, cluster_tolerance_, clusters, min_pts_per_cluster_, max_pts_per_cluster_); // Sort the clusters based on their size (largest one first) std::sort (clusters.rbegin (), clusters.rend (), comparePointClusters); deinitCompute (); }
void pcl::SeededHueSegmentation::segment (PointIndices &indices_in, PointIndices &indices_out) { if (!initCompute () || (input_ != 0 && input_->points.empty ()) || (indices_ != 0 && indices_->empty ())) { indices_out.indices.clear (); return; } // Initialize the spatial locator if (!tree_) { if (input_->isOrganized ()) tree_.reset (new pcl::search::OrganizedNeighbor<PointXYZRGB> ()); else tree_.reset (new pcl::search::KdTree<PointXYZRGB> (false)); } // Send the input dataset to the spatial locator tree_->setInputCloud (input_); seededHueSegmentation (*input_, tree_, static_cast<float> (cluster_tolerance_), indices_in, indices_out, delta_hue_); deinitCompute (); }
template <typename PointInT, typename PointOutT> void pcl::Feature<PointInT, PointOutT>::compute (PointCloudOut &output) { if (!initCompute ()) { output.width = output.height = 0; output.points.clear (); return; } // Copy the header output.header = input_->header; // Resize the output dataset if (output.points.size () != indices_->size ()) output.points.resize (indices_->size ()); // Check if the output will be computed for all points or only a subset if (indices_->size () != input_->points.size ()) { output.width = static_cast<int> (indices_->size ()); output.height = 1; } else { output.width = input_->width; output.height = input_->height; } output.is_dense = input_->is_dense; // Perform the actual feature computation computeFeature (output); deinitCompute (); }
template <typename PointInT> void pcl::MeshConstruction<PointInT>::reconstruct (std::vector<pcl::Vertices> &polygons) { if (!initCompute ()) { polygons.clear (); return; } // Check if a space search locator was given if (check_tree_) { if (!tree_) { if (input_->isOrganized ()) tree_.reset (new pcl::search::OrganizedNeighbor<PointInT> ()); else tree_.reset (new pcl::search::KdTree<PointInT> (false)); } // Send the surface dataset to the spatial locator tree_->setInputCloud (input_, indices_); } // Set up the output dataset //polygons.clear (); //polygons.reserve (2 * indices_->size ()); /// NOTE: usually the number of triangles is around twice the number of vertices // Perform the actual surface reconstruction performReconstruction (polygons); deinitCompute (); }
template <typename PointT> void pcl::LabeledEuclideanClusterExtraction<PointT>::extract (std::vector<std::vector<PointIndices> > &labeled_clusters) { if (!initCompute () || (input_ != 0 && input_->points.empty ()) || (indices_ != 0 && indices_->empty ())) { labeled_clusters.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_); extractLabeledEuclideanClusters (*input_, tree_, cluster_tolerance_, labeled_clusters, min_pts_per_cluster_, max_pts_per_cluster_, max_label_); // Sort the clusters based on their size (largest one first) for(unsigned int i = 0; i < labeled_clusters.size(); i++) std::sort (labeled_clusters[i].rbegin (), labeled_clusters[i].rend (), comparePointClusters); deinitCompute (); }
template <typename PointT> void pcl::SegmentDifferences<PointT>::segment (PointCloud &output) { output.header = input_->header; if (!initCompute ()) { output.width = output.height = 0; output.points.clear (); return; } // If target is empty, input - target = input if (target_->points.empty ()) { output = *input_; return; } // Initialize the spatial locator if (!tree_) { if (target_->isOrganized ()) tree_.reset (new pcl::OrganizedDataIndex<PointT> ()); else tree_.reset (new pcl::KdTreeFLANN<PointT> (false)); } // Send the input dataset to the spatial locator tree_->setInputCloud (target_); getPointCloudDifference (*input_, *target_, distance_threshold_, tree_, output); deinitCompute (); }
template <typename PointT> void pcl::registration::ELCH<PointT>::compute () { if (!initCompute ()) { return; } LOAGraph grb[4]; typename boost::graph_traits<LoopGraph>::edge_iterator edge_it, edge_it_end; for (boost::tuples::tie (edge_it, edge_it_end) = edges (*loop_graph_); edge_it != edge_it_end; edge_it++) { for (int j = 0; j < 4; j++) add_edge (source (*edge_it, *loop_graph_), target (*edge_it, *loop_graph_), 1, grb[j]); //TODO add variance } double *weights[4]; for (int i = 0; i < 4; i++) { weights[i] = new double[num_vertices (*loop_graph_)]; loopOptimizerAlgorithm (grb[i], weights[i]); } //TODO use pose //Eigen::Vector4f cend; //pcl::compute3DCentroid (*((*loop_graph_)[loop_end_].cloud), cend); //Eigen::Translation3f tend (cend.head (3)); //Eigen::Affine3f aend (tend); //Eigen::Affine3f aendI = aend.inverse (); //TODO iterate ovr loop_graph_ //typename boost::graph_traits<LoopGraph>::vertex_iterator vertex_it, vertex_it_end; //for (boost::tuples::tie (vertex_it, vertex_it_end) = vertices (*loop_graph_); vertex_it != vertex_it_end; vertex_it++) for (size_t i = 0; i < num_vertices (*loop_graph_); i++) { Eigen::Vector3f t2; t2[0] = loop_transform_ (0, 3) * static_cast<float> (weights[0][i]); t2[1] = loop_transform_ (1, 3) * static_cast<float> (weights[1][i]); t2[2] = loop_transform_ (2, 3) * static_cast<float> (weights[2][i]); Eigen::Affine3f bl (loop_transform_); Eigen::Quaternionf q (bl.rotation ()); Eigen::Quaternionf q2; q2 = Eigen::Quaternionf::Identity ().slerp (static_cast<float> (weights[3][i]), q); //TODO use rotation from branch start Eigen::Translation3f t3 (t2); Eigen::Affine3f a (t3 * q2); //a = aend * a * aendI; pcl::transformPointCloud (*(*loop_graph_)[i].cloud, *(*loop_graph_)[i].cloud, a); (*loop_graph_)[i].transform = a; } add_edge (loop_start_, loop_end_, *loop_graph_); deinitCompute (); }
template <typename PointSource, typename PointTarget, typename Scalar> void pcl::registration::CorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineCorrespondences ( pcl::Correspondences &correspondences, double max_distance) { if (!initCompute ()) return; double max_dist_sqr = max_distance * max_distance; typedef typename pcl::traits::fieldList<PointTarget>::type FieldListTarget; correspondences.resize (indices_->size ()); std::vector<int> index (1); std::vector<float> distance (1); pcl::Correspondence corr; unsigned int nr_valid_correspondences = 0; // Check if the template types are the same. If true, avoid a copy. // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro! if (isSamePointType<PointSource, PointTarget> ()) { // Iterate over the input set of source indices for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx) { tree_->nearestKSearch (input_->points[*idx], 1, index, distance); if (distance[0] > max_dist_sqr) continue; corr.index_query = *idx; corr.index_match = index[0]; corr.distance = distance[0]; correspondences[nr_valid_correspondences++] = corr; } } else { PointTarget pt; // Iterate over the input set of source indices for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx) { // Copy the source data to a target PointTarget format so we can search in the tree pcl::for_each_type <FieldListTarget> (pcl::NdConcatenateFunctor <PointSource, PointTarget> ( input_->points[*idx], pt)); tree_->nearestKSearch (pt, 1, index, distance); if (distance[0] > max_dist_sqr) continue; corr.index_query = *idx; corr.index_match = index[0]; corr.distance = distance[0]; correspondences[nr_valid_correspondences++] = corr; } } correspondences.resize (nr_valid_correspondences); deinitCompute (); }
template <typename PointT> bool pcl::registration::ELCH<PointT>::initCompute () { /*if (!PCLBase<PointT>::initCompute ()) { PCL_ERROR ("[pcl::registration:ELCH::initCompute] Init failed.\n"); return (false); }*/ //TODO if (loop_end_ == 0) { PCL_ERROR ("[pcl::registration::ELCH::initCompute] no end of loop defined!\n"); deinitCompute (); return (false); } //compute transformation if it's not given if (compute_loop_) { PointCloudPtr meta_start (new PointCloud); PointCloudPtr meta_end (new PointCloud); *meta_start = *(*loop_graph_)[loop_start_].cloud; *meta_end = *(*loop_graph_)[loop_end_].cloud; typename boost::graph_traits<LoopGraph>::adjacency_iterator si, si_end; for (boost::tuples::tie (si, si_end) = adjacent_vertices (loop_start_, *loop_graph_); si != si_end; si++) *meta_start += *(*loop_graph_)[*si].cloud; for (boost::tuples::tie (si, si_end) = adjacent_vertices (loop_end_, *loop_graph_); si != si_end; si++) *meta_end += *(*loop_graph_)[*si].cloud; //TODO use real pose instead of centroid //Eigen::Vector4f pose_start; //pcl::compute3DCentroid (*(*loop_graph_)[loop_start_].cloud, pose_start); //Eigen::Vector4f pose_end; //pcl::compute3DCentroid (*(*loop_graph_)[loop_end_].cloud, pose_end); PointCloudPtr tmp (new PointCloud); //Eigen::Vector4f diff = pose_start - pose_end; //Eigen::Translation3f translation (diff.head (3)); //Eigen::Affine3f trans = translation * Eigen::Quaternionf::Identity (); //pcl::transformPointCloud (*(*loop_graph_)[loop_end_].cloud, *tmp, trans); reg_->setInputTarget (meta_start); reg_->setInputCloud (meta_end); reg_->align (*tmp); loop_transform_ = reg_->getFinalTransformation (); //TODO hack //loop_transform_ *= trans.matrix (); } return (true); }
template <typename PointT, typename NormalT> void pcl::RegionGrowing<PointT, NormalT>::extract (std::vector <pcl::PointIndices>& clusters) { clusters_.clear (); clusters.clear (); point_neighbours_.clear (); point_labels_.clear (); num_pts_in_segment_.clear (); number_of_segments_ = 0; bool segmentation_is_possible = initCompute (); if ( !segmentation_is_possible ) { deinitCompute (); return; } segmentation_is_possible = prepareForSegmentation (); if ( !segmentation_is_possible ) { deinitCompute (); return; } findPointNeighbours (); applySmoothRegionGrowingAlgorithm (); assembleRegions (); clusters.resize (clusters_.size ()); std::vector<pcl::PointIndices>::iterator cluster_iter_input = clusters.begin (); for (std::vector<pcl::PointIndices>::const_iterator cluster_iter = clusters_.begin (); cluster_iter != clusters_.end (); cluster_iter++) { if ((static_cast<int> (cluster_iter->indices.size ()) >= min_pts_per_cluster_) && (static_cast<int> (cluster_iter->indices.size ()) <= max_pts_per_cluster_)) { *cluster_iter_input = *cluster_iter; cluster_iter_input++; } } clusters_ = std::vector<pcl::PointIndices> (clusters.begin (), cluster_iter_input); clusters.resize(clusters_.size()); deinitCompute (); }
template <typename PointInT, typename StateT> void pcl::tracking::Tracker<PointInT, StateT>::compute () { if (!initCompute ()) return; computeTracking (); deinitCompute (); }
template <typename PointSource, typename PointTarget> inline void pcl::Registration<PointSource, PointTarget>::align (PointCloudSource &output, const Eigen::Matrix4f& guess) { if (!initCompute ()) return; if (!target_) { PCL_WARN ("[pcl::%s::compute] No input target dataset was given!\n", getClassName ().c_str ()); return; } // Resize the output dataset if (output.points.size () != indices_->size ()) output.points.resize (indices_->size ()); // Copy the header output.header = input_->header; // Check if the output will be computed for all points or only a subset if (indices_->size () != input_->points.size ()) { output.width = (int) indices_->size (); output.height = 1; } else { output.width = input_->width; output.height = input_->height; } output.is_dense = input_->is_dense; // Copy the point data to output for (size_t i = 0; i < indices_->size (); ++i) output.points[i] = input_->points[(*indices_)[i]]; // Set the internal point representation of choice if (point_representation_) tree_->setPointRepresentation (point_representation_); // Perform the actual transformation computation converged_ = false; final_transformation_ = transformation_ = previous_transformation_ = Eigen::Matrix4f::Identity (); // Right before we estimate the transformation, we set all the point.data[3] values to 1 to aid the rigid // transformation for (size_t i = 0; i < indices_->size (); ++i) output.points[i].data[3] = 1.0; computeTransformation (output, guess); deinitCompute (); }
template <typename PointModelT, typename PointSceneT> void pcl::CorrespondenceGrouping<PointModelT, PointSceneT>::cluster (std::vector<Correspondences> &clustered_corrs) { clustered_corrs.clear (); if (!initCompute ()) { return; } //Perform the actual clustering clusterCorrespondences (clustered_corrs); deinitCompute (); }
/** \brief Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using * the surface in setSearchSurface () and the spatial locator in setSearchMethod () * \param output the resultant filtered point cloud dataset */ void pcl::Filter<sensor_msgs::PointCloud2>::filter (PointCloud2 &output) { if (!initCompute ()) return; // Copy fields and header at a minimum output.header = input_->header; output.fields = input_->fields; // Apply the actual filter applyFilter (output); deinitCompute (); }
template <typename PointSource, typename PointTarget> void pcl::registration::CorrespondenceEstimation<PointSource, PointTarget>::determineCorrespondences ( pcl::Correspondences &correspondences, float max_distance) { typedef typename pcl::traits::fieldList<PointTarget>::type FieldListTarget; if (!initCompute ()) return; if (!target_) { PCL_WARN ("[pcl::%s::compute] No input target dataset was given!\n", getClassName ().c_str ()); return; } float max_dist_sqr = max_distance * max_distance; correspondences.resize (indices_->size ()); std::vector<int> index (1); std::vector<float> distance (1); pcl::Correspondence corr; for (size_t i = 0; i < indices_->size (); ++i) { // Copy the source data to a target PointTarget format so we can search in the tree PointTarget pt; pcl::for_each_type <FieldListTarget> (pcl::NdConcatenateFunctor <PointSource, PointTarget> ( input_->points[(*indices_)[i]], pt)); //if (tree_->nearestKSearch (input_->points[(*indices_)[i]], 1, index, distance)) if (tree_->nearestKSearch (pt, 1, index, distance)) { if (distance[0] <= max_dist_sqr) { corr.index_query = static_cast<int> (i); corr.index_match = index[0]; corr.distance = distance[0]; correspondences[i] = corr; continue; } } // correspondences[i] = pcl::Correspondence(i, -1, std::numeric_limits<float>::max()); } deinitCompute (); }
template <typename PointInT, typename PointOutT> inline void pcl::Keypoint<PointInT, PointOutT>::compute (PointCloudOut &output) { if (!initCompute ()) { PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ()); return; } // Perform the actual computation detectKeypoints (output); deinitCompute (); // Reset the surface if (input_ == surface_) surface_.reset (); }
template <typename PointInT, typename PointOutT> void pcl::CloudSurfaceProcessing<PointInT, PointOutT>::process (pcl::PointCloud<PointOutT> &output) { // Copy the header output.header = input_->header; if (!initCompute ()) { output.width = output.height = 0; output.points.clear (); return; } // Perform the actual surface reconstruction performProcessing (output); deinitCompute (); }
template <typename PointInT> void pcl::ConvexHull<PointInT>::reconstruct (PointCloud &points, std::vector<pcl::Vertices> &polygons) { points.header = input_->header; if (!initCompute () || input_->points.empty () || indices_->empty ()) { points.points.clear (); return; } // Perform the actual surface reconstruction performReconstruction (points, polygons, true); points.width = static_cast<uint32_t> (points.points.size ()); points.height = 1; points.is_dense = true; deinitCompute (); }
template <typename PointT, typename Graph> void pcl::graph::OctreeAdjacencyGraphBuilder<PointT, Graph>::compute (Graph& graph) { if (!initCompute ()) { graph = Graph (); deinitCompute (); return; } if (!octree_adjacency_) { octree_adjacency_.reset (new OctreeAdjacency (voxel_resolution_)); if (with_transform_function_) octree_adjacency_->setTransformFunction (boost::bind (&OctreeAdjacencyGraphBuilder<PointT, Graph>::transformFunction, _1)); } octree_adjacency_->setInputCloud (input_, indices_); octree_adjacency_->addPointsFromInputCloud (); graph = Graph (octree_adjacency_->getLeafCount ()); leaf_vertex_map_.clear (); typename OctreeAdjacency::iterator leaf_itr = octree_adjacency_->begin (); for (VertexId v = 0; leaf_itr != octree_adjacency_->end (); ++leaf_itr, ++v) { leaf_vertex_map_[*leaf_itr] = v; copyPoint<PointT, PointOutT> ((*leaf_itr)->getData (), graph[v]); } for (leaf_itr = octree_adjacency_->begin (); leaf_itr != octree_adjacency_->end (); ++leaf_itr) { const VertexId idx1 = leaf_vertex_map_[*leaf_itr]; typename OctreeAdjacencyContainer::iterator neighb_itr; for (neighb_itr = (*leaf_itr)->begin (); neighb_itr != (*leaf_itr)->end (); ++neighb_itr) { const VertexId idx2 = leaf_vertex_map_[*neighb_itr]; if (idx1 > idx2) // this prevents from adding self-edges and duplicates boost::add_edge (idx1, idx2, graph); } } }
template <typename PointInT, typename StateT> bool pcl::tracking::Tracker<PointInT, StateT>::initCompute () { if (!PCLBase<PointInT>::initCompute ()) { PCL_ERROR ("[pcl::%s::initCompute] PCLBase::Init failed.\n", getClassName ().c_str ()); return (false); } // If the dataset is empty, just return if (input_->points.empty ()) { PCL_ERROR ("[pcl::%s::compute] input_ is empty!\n", getClassName ().c_str ()); // Cleanup deinitCompute (); return (false); } return (true); }
template <typename PointInT, typename NormalOutT> void pcl::MovingLeastSquares<PointInT, NormalOutT>::reconstruct (PointCloudIn &output) { // check if normals have to be computed/saved if (normals_) { // Copy the header normals_->header = input_->header; // Clear the fields in case the method exits before computation normals_->width = normals_->height = 0; normals_->points.clear (); } // Copy the header output.header = input_->header; if (!initCompute ()) { output.width = output.height = 0; output.points.clear (); return; } // Check if a space search locator was given if (!tree_) { PCL_ERROR ("[pcl::%s::compute] No spatial search method was given!\n", getClassName ().c_str ()); output.width = output.height = 0; output.points.clear (); return; } // Send the surface dataset to the spatial locator tree_->setInputCloud (input_, indices_); // Perform the actual surface reconstruction performReconstruction (output); deinitCompute (); }
template <typename PointInT> void pcl::SurfaceReconstruction<PointInT>::reconstruct (pcl::PolygonMesh &output) { // Copy the header output.header = input_->header; if (!initCompute ()) { output.cloud.width = output.cloud.height = 0; output.cloud.data.clear (); output.polygons.clear (); return; } // Check if a space search locator was given if (check_tree_) { if (!tree_) { if (input_->isOrganized ()) tree_.reset (new pcl::search::OrganizedNeighbor<PointInT> ()); else tree_.reset (new pcl::search::KdTree<PointInT> (false)); } // Send the surface dataset to the spatial locator tree_->setInputCloud (input_, indices_); } // Set up the output dataset pcl::toROSMsg (*input_, output.cloud); /// NOTE: passing in boost shared pointer with * as const& should be OK here output.polygons.clear (); output.polygons.reserve (2*indices_->size ()); /// NOTE: usually the number of triangles is around twice the number of vertices // Perform the actual surface reconstruction performReconstruction (output); deinitCompute (); }
template <typename PointInT, typename PointOutT> void pcl::Feature<PointInT, PointOutT>::computeEigen (pcl::PointCloud<Eigen::MatrixXf> &output) { if (!initCompute ()) { output.width = output.height = 0; output.points.resize (0, 0); return; } // Copy the properties //#ifndef USE_ROS // output.properties.acquisition_time = input_->header.stamp; //#endif output.properties.sensor_origin = input_->sensor_origin_; output.properties.sensor_orientation = input_->sensor_orientation_; // Check if the output will be computed for all points or only a subset if (indices_->size () != input_->points.size ()) { output.width = static_cast<int> (indices_->size ()); output.height = 1; } else { output.width = input_->width; output.height = input_->height; } output.is_dense = input_->is_dense; // Perform the actual feature computation computeFeatureEigen (output); deinitCompute (); }
template <typename PointInT, typename PointOutT> void pcl::BilateralUpsampling<PointInT, PointOutT>::process (pcl::PointCloud<PointOutT> &output) { // Copy the header output.header = input_->header; if (!initCompute ()) { output.width = output.height = 0; output.points.clear (); return; } if (input_->isOrganized () == false) { PCL_ERROR ("Input cloud is not organized.\n"); return; } // Invert projection matrix unprojection_matrix_ = projection_matrix_.inverse (); for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) printf ("%f ", unprojection_matrix_(i, j)); printf ("\n"); } // Perform the actual surface reconstruction performProcessing (output); deinitCompute (); }
template <typename PointInT, typename PointOutT> void pcl::MovingLeastSquares<PointInT, PointOutT>::process (PointCloudOut &output) { // Check if normals have to be computed/saved if (compute_normals_) { normals_.reset (new NormalCloud); // Copy the header normals_->header = input_->header; // Clear the fields in case the method exits before computation normals_->width = normals_->height = 0; normals_->points.clear (); } // Copy the header output.header = input_->header; output.width = output.height = 0; output.points.clear (); if (search_radius_ <= 0 || sqr_gauss_param_ <= 0) { PCL_ERROR ("[pcl::%s::reconstruct] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_); return; } if (!initCompute ()) return; // Initialize the spatial locator if (!tree_) { KdTreePtr tree; if (input_->isOrganized ()) tree.reset (new pcl::search::OrganizedNeighbor<PointInT> ()); else tree.reset (new pcl::search::KdTree<PointInT> (false)); setSearchMethod (tree); } // Send the surface dataset to the spatial locator tree_->setInputCloud (input_, indices_); // Initialize random number generator if necessary switch (upsample_method_) { case (RANDOM_UNIFORM_DENSITY): { boost::mt19937 *rng = new boost::mt19937 (static_cast<unsigned int>(std::time(0))); float tmp = static_cast<float> (search_radius_ / 2.0f); boost::uniform_real<float> *uniform_distrib = new boost::uniform_real<float> (-tmp, tmp); rng_uniform_distribution_ = new boost::variate_generator<boost::mt19937, boost::uniform_real<float> > (*rng, *uniform_distrib); break; } case (VOXEL_GRID_DILATION): { mls_results_.resize (input_->size ()); break; } default: break; } // Perform the actual surface reconstruction performProcessing (output); if (compute_normals_) { normals_->height = 1; normals_->width = static_cast<uint32_t> (normals_->size ()); // TODO!!! MODIFY TO PER-CLOUD COPYING - much faster than per-point for (unsigned int i = 0; i < output.size (); ++i) { typedef typename pcl::traits::fieldList<PointOutT>::type FieldList; pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_x", normals_->points[i].normal_x)); pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_y", normals_->points[i].normal_y)); pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_z", normals_->points[i].normal_z)); pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "curvature", normals_->points[i].curvature)); } } // Set proper widths and heights for the clouds output.height = 1; output.width = static_cast<uint32_t> (output.size ()); deinitCompute (); }
template<typename PointT> void pcl::ConditionalEuclideanClustering<PointT>::segment (pcl::IndicesClusters &clusters) { // Prepare output (going to use push_back) clusters.clear (); if (extract_removed_clusters_) { small_clusters_->clear (); large_clusters_->clear (); } // Validity checks if (!initCompute () || input_->points.empty () || indices_->empty () || !condition_function_) 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> ()); } searcher_->setInputCloud (input_, indices_); // Temp variables used by search class std::vector<int> nn_indices; std::vector<float> nn_distances; // Create a bool vector of processed point indices, and initialize it to false // Need to have it contain all possible points because radius search can not return indices into indices std::vector<bool> processed (input_->points.size (), false); // Process all points indexed by indices_ for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii) // iii = input indices iterator { // Has this point been processed before? if ((*indices_)[iii] == -1 || processed[(*indices_)[iii]]) continue; // Set up a new growing cluster std::vector<int> current_cluster; int cii = 0; // cii = cluster indices iterator // Add the point to the cluster current_cluster.push_back ((*indices_)[iii]); processed[(*indices_)[iii]] = true; // Process the current cluster (it can be growing in size as it is being processed) while (cii < static_cast<int> (current_cluster.size ())) { // Search for neighbors around the current seed point of the current cluster if (searcher_->radiusSearch (input_->points[current_cluster[cii]], cluster_tolerance_, nn_indices, nn_distances) < 1) { cii++; continue; } // Process the neighbors for (int nii = 1; nii < static_cast<int> (nn_indices.size ()); ++nii) // nii = neighbor indices iterator { // Has this point been processed before? if (nn_indices[nii] == -1 || processed[nn_indices[nii]]) continue; // Validate if condition holds if (condition_function_ (input_->points[current_cluster[cii]], input_->points[nn_indices[nii]], nn_distances[nii])) { // Add the point to the cluster current_cluster.push_back (nn_indices[nii]); processed[nn_indices[nii]] = true; } } cii++; } // If extracting removed clusters, all clusters need to be saved, otherwise only the ones within the given cluster size range if (extract_removed_clusters_ || (current_cluster.size () >= min_cluster_size_ && current_cluster.size () <= max_cluster_size_)) { pcl::PointIndices pi; pi.header = input_->header; pi.indices.resize (current_cluster.size ()); for (int ii = 0; ii < static_cast<int> (current_cluster.size ()); ++ii) // ii = indices iterator pi.indices[ii] = current_cluster[ii]; if (extract_removed_clusters_ && current_cluster.size () < min_cluster_size_) small_clusters_->push_back (pi); else if (extract_removed_clusters_ && current_cluster.size () > max_cluster_size_) large_clusters_->push_back (pi); else clusters.push_back (pi); } } deinitCompute (); }
template<typename PointT, typename PointNT, typename PointLT> void pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segment (std::vector<ModelCoefficients>& model_coefficients, std::vector<PointIndices>& inlier_indices, std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& centroids, std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >& covariances, pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) { if (!initCompute ()) return; // Check that we got the same number of points and normals if (static_cast<int> (normals_->points.size ()) != static_cast<int> (input_->points.size ())) { PCL_ERROR ("[pcl::%s::segment] Number of points in input cloud (%zu) and normal cloud (%zu) do not match!\n", getClassName ().c_str (), input_->points.size (), normals_->points.size ()); return; } // Check that the cloud is organized if (!input_->isOrganized ()) { PCL_ERROR ("[pcl::%s::segment] Organized point cloud is required for this plane extraction method!\n", getClassName ().c_str ()); return; } // Calculate range part of planes' hessian normal form std::vector<float> plane_d (input_->points.size ()); for (unsigned int i = 0; i < input_->size (); ++i) plane_d[i] = input_->points[i].getVector3fMap ().dot (normals_->points[i].getNormalVector3fMap ()); // Make a comparator //PlaneCoefficientComparator<PointT,PointNT> plane_comparator (plane_d); compare_->setPlaneCoeffD (plane_d); compare_->setInputCloud (input_); compare_->setInputNormals (normals_); compare_->setAngularThreshold (static_cast<float> (angular_threshold_)); compare_->setDistanceThreshold (static_cast<float> (distance_threshold_), true); // Set up the output OrganizedConnectedComponentSegmentation<PointT,pcl::Label> connected_component (compare_); connected_component.setInputCloud (input_); connected_component.segment (labels, label_indices); Eigen::Vector4f clust_centroid = Eigen::Vector4f::Zero (); Eigen::Vector4f vp = Eigen::Vector4f::Zero (); Eigen::Matrix3f clust_cov; pcl::ModelCoefficients model; model.values.resize (4); // Fit Planes to each cluster for (size_t i = 0; i < label_indices.size (); i++) { if (static_cast<unsigned> (label_indices[i].indices.size ()) > min_inliers_) { pcl::computeMeanAndCovarianceMatrix (*input_, label_indices[i].indices, clust_cov, clust_centroid); Eigen::Vector4f plane_params; EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value; EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; pcl::eigen33 (clust_cov, eigen_value, eigen_vector); plane_params[0] = eigen_vector[0]; plane_params[1] = eigen_vector[1]; plane_params[2] = eigen_vector[2]; plane_params[3] = 0; plane_params[3] = -1 * plane_params.dot (clust_centroid); vp -= clust_centroid; float cos_theta = vp.dot (plane_params); if (cos_theta < 0) { plane_params *= -1; plane_params[3] = 0; plane_params[3] = -1 * plane_params.dot (clust_centroid); } // Compute the curvature surface change float curvature; float eig_sum = clust_cov.coeff (0) + clust_cov.coeff (4) + clust_cov.coeff (8); if (eig_sum != 0) curvature = fabsf (eigen_value / eig_sum); else curvature = 0; if (curvature < maximum_curvature_) { model.values[0] = plane_params[0]; model.values[1] = plane_params[1]; model.values[2] = plane_params[2]; model.values[3] = plane_params[3]; model_coefficients.push_back (model); inlier_indices.push_back (label_indices[i]); centroids.push_back (clust_centroid); covariances.push_back (clust_cov); } } } deinitCompute (); }