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