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
66 calculateConvexConnections (sv_adjacency_list_);
69 applyKconvexity (k_factor_);
74 grouping_data_valid_ =
true;
76 applyCuttingPlane (max_cuts_);
79 mergeSmallSegments ();
82 PCL_WARN (
"[pcl::CPCSegmentation::segment] WARNING: Call function setInputSupervoxels first. Nothing has been done. \n");
85 template <
typename Po
intT>
void
88 using SegLabel2ClusterMap = std::map<std::uint32_t, pcl::PointCloud<WeightSACPointType>::Ptr>;
92 if (depth_levels_left <= 0)
96 SegLabel2ClusterMap seg_to_edge_points_map;
97 std::map<std::uint32_t, std::vector<EdgeID> > seg_to_edgeIDs_map;
98 EdgeIterator edge_itr, edge_itr_end, next_edge;
99 boost::tie (edge_itr, edge_itr_end) = boost::edges (sv_adjacency_list_);
100 for (next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge)
103 std::uint32_t source_sv_label = sv_adjacency_list_[boost::source (*edge_itr, sv_adjacency_list_)];
104 std::uint32_t target_sv_label = sv_adjacency_list_[boost::target (*edge_itr, sv_adjacency_list_)];
106 std::uint32_t source_segment_label = sv_label_to_seg_label_map_[source_sv_label];
107 std::uint32_t target_segment_label = sv_label_to_seg_label_map_[target_sv_label];
110 if (source_segment_label != target_segment_label)
114 if (sv_adjacency_list_[*edge_itr].used_for_cutting)
117 const pcl::PointXYZRGBA source_centroid = sv_label_to_supervoxel_map_[source_sv_label]->centroid_;
118 const pcl::PointXYZRGBA target_centroid = sv_label_to_supervoxel_map_[target_sv_label]->centroid_;
123 WeightSACPointType edge_centroid;
124 edge_centroid.getVector3fMap () = (source_centroid.getVector3fMap () + target_centroid.getVector3fMap ()) / 2;
127 edge_centroid.getNormalVector3fMap () = (target_centroid.getVector3fMap () - source_centroid.getVector3fMap ()).normalized ();
130 edge_centroid.intensity = sv_adjacency_list_[*edge_itr].is_convex ? -sv_adjacency_list_[*edge_itr].normal_difference : sv_adjacency_list_[*edge_itr].normal_difference;
131 if (seg_to_edge_points_map.find (source_segment_label) == seg_to_edge_points_map.end ())
135 seg_to_edge_points_map[source_segment_label]->push_back (edge_centroid);
136 seg_to_edgeIDs_map[source_segment_label].push_back (*edge_itr);
138 bool cut_found =
false;
140 for (
const auto &seg_to_edge_points : seg_to_edge_points_map)
143 if (seg_to_edge_points.second->size () < min_segment_size_for_cutting_)
148 std::vector<double> weights;
149 weights.resize (seg_to_edge_points.second->size ());
150 for (std::size_t cp = 0;
cp < seg_to_edge_points.second->size (); ++
cp)
152 float& cur_weight = seg_to_edge_points.second->points[
cp].intensity;
153 cur_weight = cur_weight < concavity_tolerance_threshold_ ? 0 : 1;
154 weights[
cp] = cur_weight;
160 WeightedRandomSampleConsensus weight_sac (model_p, seed_resolution_,
true);
162 weight_sac.setWeights (weights, use_directed_weights_);
163 weight_sac.setMaxIterations (ransac_itrs_);
166 if (!weight_sac.computeModel ())
171 Eigen::VectorXf model_coefficients;
172 weight_sac.getModelCoefficients (model_coefficients);
174 model_coefficients[3] += std::numeric_limits<float>::epsilon ();
176 weight_sac.getInliers (*support_indices);
179 pcl::Indices 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;
190 tree->setInputCloud (edge_cloud_cluster);
196 euclidean_clusterer.
setIndices (support_indices);
197 euclidean_clusterer.
extract (cluster_indices);
200 for (
const auto &cluster_index : cluster_indices)
203 int cluster_concave_pts = 0;
204 float cluster_score = 0;
206 for (
const int ¤t_index : cluster_index.indices)
208 double index_score = weights[current_index];
209 if (use_directed_weights_)
210 index_score *= 1.414 * (std::abs (plane_normal.dot (edge_cloud_cluster->
at (current_index).getNormalVector3fMap ())));
211 cluster_score += index_score;
212 if (weights[current_index] > 0)
213 ++cluster_concave_pts;
216 cluster_score /= cluster_index.indices.size ();
218 if (cluster_score >= min_cut_score_)
220 cut_support_indices.insert (cut_support_indices.end (), cluster_index.indices.begin (), cluster_index.indices.end ());
223 if (cut_support_indices.empty ())
231 double current_score = weight_sac.getBestScore ();
232 cut_support_indices = *support_indices;
234 if (current_score < min_cut_score_)
241 int number_connections_cut = 0;
242 for (
const int &point_index : cut_support_indices)
244 if (use_clean_cutting_)
247 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_)];
248 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_)];
250 const pcl::PointXYZRGBA source_centroid = sv_label_to_supervoxel_map_[source_sv_label]->centroid_;
251 const pcl::PointXYZRGBA target_centroid = sv_label_to_supervoxel_map_[target_sv_label]->centroid_;
258 sv_adjacency_list_[seg_to_edgeIDs_map[seg_to_edge_points.first][point_index]].used_for_cutting =
true;
259 if (sv_adjacency_list_[seg_to_edgeIDs_map[seg_to_edge_points.first][point_index]].is_valid)
261 ++number_connections_cut;
262 sv_adjacency_list_[seg_to_edgeIDs_map[seg_to_edge_points.first][point_index]].is_valid =
false;
266 if (number_connections_cut > 0)
275 applyCuttingPlane (depth_levels_left);
284 template <
typename Po
intT>
bool
288 if (threshold_ == std::numeric_limits<double>::max ())
290 PCL_ERROR (
"[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] No threshold set!\n");
295 best_score_ = -std::numeric_limits<double>::max ();
297 std::vector<int> selection;
298 Eigen::VectorXf model_coefficients;
300 unsigned skipped_count = 0;
302 const unsigned max_skip = max_iterations_ * 10;
305 while (iterations_ < max_iterations_ && skipped_count < max_skip)
308 sac_model_->setIndices (model_pt_indices_);
309 sac_model_->getSamples (iterations_, selection);
311 if (selection.empty ())
313 PCL_ERROR (
"[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] No samples could be selected!\n");
318 if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
325 sac_model_->setIndices (full_cloud_pt_indices_);
328 sac_model_->selectWithinDistance (model_coefficients, threshold_, *current_inliers);
329 double current_score = 0;
330 Eigen::Vector3f plane_normal (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
331 for (
const int ¤t_index : *current_inliers)
333 double index_score = weights_[current_index];
334 if (use_directed_weights_)
336 index_score *= 1.414 * (std::abs (plane_normal.dot (point_cloud_ptr_->at (current_index).getNormalVector3fMap ())));
338 current_score += index_score;
341 current_score /= current_inliers->size ();
344 if (current_score > best_score_)
346 best_score_ = current_score;
349 model_coefficients_ = model_coefficients;
353 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_);
354 if (iterations_ > max_iterations_)
356 PCL_DEBUG (
"[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] RANSAC reached the maximum number of trials.\n");
361 PCL_DEBUG (
"[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] Model: %lu size, %f score.\n", model_.size (), best_score_);
370 sac_model_->selectWithinDistance (model_coefficients_, threshold_, inliers_);
374 #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.
shared_ptr< KdTree< PointT, Tree > > Ptr
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
shared_ptr< SampleConsensusModelPlane< PointT > > Ptr
void segment()
Merge supervoxels using cuts through local convexities.
PCL_EXPORTS void print_info(const char *format,...)
Print an info message on stream with colors.
shared_ptr< Indices > IndicesPtr
A segmentation algorithm partitioning a supervoxel graph.
A point structure representing Euclidean xyz coordinates, and the RGBA color.
shared_ptr< PointCloud< PointT > > Ptr
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. ...
int cp(int from, int to)
Returns field copy operation code.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
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.