38 #ifndef PCL_SEGMENTATION_IMPL_CPC_SEGMENTATION_HPP_ 39 #define PCL_SEGMENTATION_IMPL_CPC_SEGMENTATION_HPP_ 41 #include <pcl/segmentation/cpc_segmentation.h> 43 template <
typename Po
intT>
46 min_segment_size_for_cutting_ (400),
47 min_cut_score_ (0.16),
48 use_local_constrains_ (true),
49 use_directed_weights_ (true),
54 template <
typename Po
intT>
59 template <
typename Po
intT>
void 76 applyCuttingPlane (max_cuts_);
82 PCL_WARN (
"[pcl::CPCSegmentation::segment] WARNING: Call function setInputSupervoxels first. Nothing has been done. \n");
85 template <
typename Po
intT>
void 88 typedef std::map<uint32_t, pcl::PointCloud<WeightSACPointType>::Ptr> SegLabel2ClusterMap;
92 if (depth_levels_left <= 0)
95 SegLabel2ClusterMap seg_to_edge_points_map;
96 std::map<uint32_t, std::vector<EdgeID> > seg_to_edgeIDs_map;
97 EdgeIterator edge_itr, edge_itr_end, next_edge;
99 for (next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge)
109 if (source_segment_label != target_segment_label)
123 edge_centroid.getVector3fMap () = (source_centroid.getVector3fMap () + target_centroid.getVector3fMap ()) / 2;
126 edge_centroid.getNormalVector3fMap () = (target_centroid.getVector3fMap () - source_centroid.getVector3fMap ()).normalized ();
130 if (seg_to_edge_points_map.find (source_segment_label) == seg_to_edge_points_map.end ())
134 seg_to_edge_points_map[source_segment_label]->push_back (edge_centroid);
135 seg_to_edgeIDs_map[source_segment_label].push_back (*edge_itr);
137 bool cut_found =
false;
139 for (SegLabel2ClusterMap::iterator itr = seg_to_edge_points_map.begin (); itr != seg_to_edge_points_map.end (); ++itr)
142 if (itr->second->size () < min_segment_size_for_cutting_)
147 std::vector<double> weights;
148 weights.resize (itr->second->size ());
149 for (std::size_t cp = 0; cp < itr->second->size (); ++cp)
151 float& cur_weight = itr->second->points[cp].intensity;
153 weights[cp] = cur_weight;
161 weight_sac.setWeights (weights, use_directed_weights_);
162 weight_sac.setMaxIterations (ransac_itrs_);
165 if (!weight_sac.computeModel ())
170 Eigen::VectorXf model_coefficients;
171 weight_sac.getModelCoefficients (model_coefficients);
173 model_coefficients[3] += std::numeric_limits<float>::epsilon ();
175 std::vector<int> support_indices;
176 weight_sac.getInliers (support_indices);
179 std::vector<int> cut_support_indices;
181 if (use_local_constrains_)
183 Eigen::Vector3f plane_normal (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
187 std::vector<pcl::PointIndices> cluster_indices;
196 euclidean_clusterer.
setIndices (boost::make_shared <std::vector <int> > (support_indices));
197 euclidean_clusterer.
extract (cluster_indices);
200 for (
size_t cc = 0; cc < cluster_indices.size (); ++cc)
203 int cluster_concave_pts = 0;
204 float cluster_score = 0;
206 for (
size_t cp = 0; cp < cluster_indices[cc].indices.size (); ++cp)
208 int current_index = cluster_indices[cc].indices[cp];
210 if (use_directed_weights_)
211 index_score = weights[current_index] * 1.414 * (fabsf (plane_normal.dot (edge_cloud_cluster->
at (current_index).getNormalVector3fMap ())));
213 index_score = weights[current_index];
214 cluster_score += index_score;
215 if (weights[current_index] > 0)
216 ++cluster_concave_pts;
219 cluster_score = cluster_score * 1.0 / cluster_indices[cc].indices.size ();
221 if (cluster_score >= min_cut_score_)
223 cut_support_indices.insert (cut_support_indices.end (), cluster_indices[cc].indices.begin (), cluster_indices[cc].indices.end ());
226 if (cut_support_indices.size () == 0)
234 double current_score = weight_sac.getBestScore ();
235 cut_support_indices = support_indices;
237 if (current_score < min_cut_score_)
244 int number_connections_cut = 0;
245 for (
size_t cs = 0; cs < cut_support_indices.size (); ++cs)
247 const int point_index = cut_support_indices[cs];
249 if (use_clean_cutting_)
263 sv_adjacency_list_[seg_to_edgeIDs_map[itr->first][point_index]].used_for_cutting =
true;
266 ++number_connections_cut;
271 if (number_connections_cut > 0)
280 applyCuttingPlane (depth_levels_left);
289 template <
typename Po
intT>
bool 293 if (threshold_ == std::numeric_limits<double>::max ())
295 PCL_ERROR (
"[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] No threshold set!\n");
300 best_score_ = -std::numeric_limits<double>::max ();
302 std::vector<int> selection;
303 Eigen::VectorXf model_coefficients;
305 unsigned skipped_count = 0;
307 const unsigned max_skip = max_iterations_ * 10;
310 while (iterations_ < max_iterations_ && skipped_count < max_skip)
313 sac_model_->setIndices (model_pt_indices_);
314 sac_model_->getSamples (iterations_, selection);
316 if (selection.empty ())
318 PCL_ERROR (
"[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] No samples could be selected!\n");
323 if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
330 sac_model_->setIndices (full_cloud_pt_indices_);
332 boost::shared_ptr<std::vector<int> > current_inliers (
new std::vector<int>);
333 sac_model_->selectWithinDistance (model_coefficients, threshold_, *current_inliers);
334 double current_score = 0;
335 Eigen::Vector3f plane_normal (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
336 for (
size_t i = 0; i < current_inliers->size (); ++i)
338 int current_index = current_inliers->at (i);
340 if (use_directed_weights_)
342 index_score = weights_[current_index] * 1.414 * (fabsf (plane_normal.dot (point_cloud_ptr_->at (current_index).getNormalVector3fMap ())));
344 index_score = weights_[current_index];
346 current_score += index_score;
349 current_score = current_score * 1.0 / current_inliers->size ();
352 if (current_score > best_score_)
354 best_score_ = current_score;
357 model_coefficients_ = model_coefficients;
361 PCL_DEBUG (
"[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] Trial %d (max %d): score is %f (best is: %f so far).\n", iterations_, max_iterations_, current_score, best_score_);
362 if (iterations_ > max_iterations_)
364 PCL_DEBUG (
"[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] RANSAC reached the maximum number of trials.\n");
369 PCL_DEBUG (
"[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] Model: %lu size, %f score.\n", model_.size (), best_score_);
378 sac_model_->selectWithinDistance (model_coefficients_, threshold_, inliers_);
382 #endif // PCL_SEGMENTATION_IMPL_CPC_SEGMENTATION_HPP_ double pointToPlaneDistanceSigned(const Point &p, double a, double b, double c, double d)
Get the distance from a point to a plane (signed) defined by ax+by+cz+d=0.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
void doGrouping()
Perform depth search on the graph and recursively group all supervoxels with convex connections...
void segment()
Merge supervoxels using cuts through local convexities.
boost::shared_ptr< SampleConsensusModelPlane > Ptr
PCL_EXPORTS void print_info(const char *format,...)
Print an info message on stream with colors.
A segmentation algorithm partitioning a supervoxel graph.
A point structure representing Euclidean xyz coordinates, and the RGBA color.
uint32_t k_factor_
Factor used for k-convexity.
boost::shared_ptr< PointCloud< PointT > > Ptr
std::map< uint32_t, typename pcl::Supervoxel< PointT >::Ptr > sv_label_to_supervoxel_map_
map from the supervoxel labels to the supervoxel objects
float seed_resolution_
Seed resolution of the supervoxels (used only for smoothness check)
boost::shared_ptr< KdTree< PointT, Tree > > Ptr
A point structure representing Euclidean xyz coordinates, intensity, together with normal coordinates...
bool supervoxels_set_
Marks if supervoxels have been set by calling setInputSupervoxels.
void mergeSmallSegments()
Segments smaller than min_segment_size_ are merged to the label of largest neighbor.
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
virtual ~CPCSegmentation()
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
std::map< uint32_t, uint32_t > sv_label_to_seg_label_map_
Storing relation between original SuperVoxel Labels and new segmantion labels.
float concavity_tolerance_threshold_
*** Parameters *** ///
void calculateConvexConnections(SupervoxelAdjacencyList &adjacency_list_arg)
Calculates convexity of edges and saves this to the adjacency graph.
void applyKconvexity(const unsigned int k_arg)
Connections are only convex if this is true for at least k_arg common neighbors of the two patches...
const PointT & at(int column, int row) const
Obtain the point given by the (column, row) coordinates.
SampleConsensusModelPlane defines a model for 3D plane segmentation.
bool grouping_data_valid_
Marks if valid grouping data (sv_adjacency_list_, sv_label_to_seg_label_map_, processed_) is avaiable...
SupervoxelAdjacencyList sv_adjacency_list_
Adjacency graph with the supervoxel labels as nodes and edges between adjacent supervoxels.