38 #ifndef PCL_SEGMENTATION_IMPL_CPC_SEGMENTATION_HPP_
39 #define PCL_SEGMENTATION_IMPL_CPC_SEGMENTATION_HPP_
41 #include <pcl/sample_consensus/sac_model_plane.h>
42 #include <pcl/segmentation/cpc_segmentation.h>
44 template <
typename Po
intT>
47 min_segment_size_for_cutting_ (400),
48 min_cut_score_ (0.16),
49 use_local_constrains_ (true),
50 use_directed_weights_ (true),
55 template <
typename Po
intT>
58 template <
typename Po
intT>
void
65 calculateConvexConnections (sv_adjacency_list_);
68 applyKconvexity (k_factor_);
73 grouping_data_valid_ =
true;
75 applyCuttingPlane (max_cuts_);
78 mergeSmallSegments ();
81 PCL_WARN (
"[pcl::CPCSegmentation::segment] WARNING: Call function setInputSupervoxels first. Nothing has been done. \n");
84 template <
typename Po
intT>
void
87 using SegLabel2ClusterMap = std::map<std::uint32_t, pcl::PointCloud<WeightSACPointType>::Ptr>;
91 if (depth_levels_left <= 0)
95 SegLabel2ClusterMap seg_to_edge_points_map;
96 std::map<std::uint32_t, std::vector<EdgeID> > seg_to_edgeIDs_map;
97 EdgeIterator edge_itr, edge_itr_end, next_edge;
98 boost::tie (edge_itr, edge_itr_end) = boost::edges (sv_adjacency_list_);
99 for (next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge)
102 std::uint32_t source_sv_label = sv_adjacency_list_[boost::source (*edge_itr, sv_adjacency_list_)];
103 std::uint32_t target_sv_label = sv_adjacency_list_[boost::target (*edge_itr, sv_adjacency_list_)];
105 std::uint32_t source_segment_label = sv_label_to_seg_label_map_[source_sv_label];
106 std::uint32_t target_segment_label = sv_label_to_seg_label_map_[target_sv_label];
109 if (source_segment_label != target_segment_label)
113 if (sv_adjacency_list_[*edge_itr].used_for_cutting)
116 const pcl::PointXYZRGBA source_centroid = sv_label_to_supervoxel_map_[source_sv_label]->centroid_;
117 const pcl::PointXYZRGBA target_centroid = sv_label_to_supervoxel_map_[target_sv_label]->centroid_;
122 WeightSACPointType edge_centroid;
123 edge_centroid.getVector3fMap () = (source_centroid.getVector3fMap () + target_centroid.getVector3fMap ()) / 2;
126 edge_centroid.getNormalVector3fMap () = (target_centroid.getVector3fMap () - source_centroid.getVector3fMap ()).normalized ();
129 edge_centroid.intensity = sv_adjacency_list_[*edge_itr].is_convex ? -sv_adjacency_list_[*edge_itr].normal_difference : sv_adjacency_list_[*edge_itr].normal_difference;
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 (
const auto &seg_to_edge_points : seg_to_edge_points_map)
142 if (seg_to_edge_points.second->size () < min_segment_size_for_cutting_)
147 std::vector<double> weights;
148 weights.resize (seg_to_edge_points.second->size ());
149 for (std::size_t cp = 0;
cp < seg_to_edge_points.second->size (); ++
cp)
151 float& cur_weight = (*seg_to_edge_points.second)[cp].intensity;
152 cur_weight = cur_weight < concavity_tolerance_threshold_ ? 0 : 1;
153 weights[
cp] = cur_weight;
159 WeightedRandomSampleConsensus weight_sac (model_p, seed_resolution_,
true);
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 weight_sac.getInliers (*support_indices);
180 if (use_local_constrains_)
182 Eigen::Vector3f plane_normal (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
186 std::vector<pcl::PointIndices> cluster_indices;
189 tree->setInputCloud (edge_cloud_cluster);
195 euclidean_clusterer.
setIndices (support_indices);
196 euclidean_clusterer.
extract (cluster_indices);
199 for (
const auto &cluster_index : cluster_indices)
202 float cluster_score = 0;
204 for (
const auto ¤t_index : cluster_index.indices)
206 double index_score = weights[current_index];
207 if (use_directed_weights_)
208 index_score *= 1.414 * (std::abs (plane_normal.dot (edge_cloud_cluster->
at (current_index).getNormalVector3fMap ())));
209 cluster_score += index_score;
212 cluster_score /= cluster_index.indices.size ();
214 if (cluster_score >= min_cut_score_)
216 cut_support_indices.insert (cut_support_indices.end (), cluster_index.indices.begin (), cluster_index.indices.end ());
219 if (cut_support_indices.empty ())
227 double current_score = weight_sac.getBestScore ();
228 cut_support_indices = *support_indices;
230 if (current_score < min_cut_score_)
237 int number_connections_cut = 0;
238 for (
const auto &point_index : cut_support_indices)
240 if (use_clean_cutting_)
243 std::uint32_t source_sv_label = sv_adjacency_list_[boost::source (seg_to_edgeIDs_map[seg_to_edge_points.first][point_index], sv_adjacency_list_)];
244 std::uint32_t target_sv_label = sv_adjacency_list_[boost::target (seg_to_edgeIDs_map[seg_to_edge_points.first][point_index], sv_adjacency_list_)];
246 const pcl::PointXYZRGBA source_centroid = sv_label_to_supervoxel_map_[source_sv_label]->centroid_;
247 const pcl::PointXYZRGBA target_centroid = sv_label_to_supervoxel_map_[target_sv_label]->centroid_;
254 sv_adjacency_list_[seg_to_edgeIDs_map[seg_to_edge_points.first][point_index]].used_for_cutting =
true;
255 if (sv_adjacency_list_[seg_to_edgeIDs_map[seg_to_edge_points.first][point_index]].is_valid)
257 ++number_connections_cut;
258 sv_adjacency_list_[seg_to_edgeIDs_map[seg_to_edge_points.first][point_index]].is_valid =
false;
262 if (number_connections_cut > 0)
271 applyCuttingPlane (depth_levels_left);
280 template <
typename Po
intT>
bool
284 if (threshold_ == std::numeric_limits<double>::max ())
286 PCL_ERROR (
"[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] No threshold set!\n");
291 best_score_ = -std::numeric_limits<double>::max ();
294 Eigen::VectorXf model_coefficients;
296 unsigned skipped_count = 0;
298 const unsigned max_skip = max_iterations_ * 10;
301 while (iterations_ < max_iterations_ && skipped_count < max_skip)
304 sac_model_->setIndices (model_pt_indices_);
305 sac_model_->getSamples (iterations_, selection);
307 if (selection.empty ())
309 PCL_ERROR (
"[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] No samples could be selected!\n");
314 if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
321 sac_model_->setIndices (full_cloud_pt_indices_);
324 sac_model_->selectWithinDistance (model_coefficients, threshold_, *current_inliers);
325 double current_score = 0;
326 Eigen::Vector3f plane_normal (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
327 for (
const auto ¤t_index : *current_inliers)
329 double index_score = weights_[current_index];
330 if (use_directed_weights_)
332 index_score *= 1.414 * (std::abs (plane_normal.dot (point_cloud_ptr_->at (current_index).getNormalVector3fMap ())));
334 current_score += index_score;
337 current_score /= current_inliers->size ();
340 if (current_score > best_score_)
342 best_score_ = current_score;
345 model_coefficients_ = model_coefficients;
349 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_);
350 if (iterations_ > max_iterations_)
352 PCL_DEBUG (
"[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] RANSAC reached the maximum number of trials.\n");
357 PCL_DEBUG (
"[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] Model: %lu size, %f score.\n", model_.size (), best_score_);
366 sac_model_->selectWithinDistance (model_coefficients_, threshold_, inliers_);
A segmentation algorithm partitioning a supervoxel graph.
void segment()
Merge supervoxels using cuts through local convexities.
~CPCSegmentation() override
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
PointCloud represents the base class in PCL for storing collections of 3D points.
const PointT & at(int column, int row) const
Obtain the point given by the (column, row) coordinates.
shared_ptr< PointCloud< PointT > > Ptr
SampleConsensusModelPlane defines a model for 3D plane segmentation.
shared_ptr< SampleConsensusModelPlane< PointT > > Ptr
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
shared_ptr< KdTree< PointT, Tree > > Ptr
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.
PCL_EXPORTS void print_info(const char *format,...)
Print an info message on stream with colors.
int cp(int from, int to)
Returns field copy operation code.
IndicesAllocator<> Indices
Type used for indices in PCL.
shared_ptr< Indices > IndicesPtr
A point structure representing Euclidean xyz coordinates, and the RGBA color.