41 #include <pcl/common/point_tests.h> 42 #include <pcl/search/organized.h> 43 #include <pcl/search/kdtree.h> 53 return ((c1.
r-c2.
r)*(c1.
r-c2.
r)+(c1.
g-c2.
g)*(c1.
g-c2.
g)+(c1.
b-c2.
b)*(c1.
b-c2.
b));
56 namespace segmentation
59 template <
typename Po
intT>
62 r =
static_cast<float> (p.r) / 255.0;
63 g =
static_cast<float> (p.g) / 255.0;
64 b =
static_cast<float> (p.b) / 255.0;
67 template <
typename Po
intT>
68 grabcut::Color::operator
PointT ()
const 79 template <
typename Po
intT>
void 85 template <
typename Po
intT>
bool 91 PCL_ERROR (
"[pcl::GrabCut::initCompute ()] Init failed!");
95 std::vector<pcl::PCLPointField> in_fields_;
96 if ((pcl::getFieldIndex<PointT> (
"rgb", in_fields_) == -1) &&
97 (pcl::getFieldIndex<PointT> (
"rgba", in_fields_) == -1))
99 PCL_ERROR (
"[pcl::GrabCut::initCompute ()] No RGB data available, aborting!");
104 image_.reset (
new Image (input_->width, input_->height));
105 for (std::size_t i = 0; i < input_->size (); ++i)
107 (*image_) [i] =
Color ((*input_)[i]);
109 width_ = image_->width;
110 height_ = image_->height;
113 if (!tree_ && !input_->isOrganized ())
116 tree_->setInputCloud (input_);
119 const std::size_t indices_size = indices_->size ();
120 trimap_ = std::vector<segmentation::grabcut::TrimapValue> (indices_size,
TrimapUnknown);
121 hard_segmentation_ = std::vector<segmentation::grabcut::SegmentationValue> (indices_size,
123 GMM_component_.resize (indices_size);
124 n_links_.resize (indices_size);
127 foreground_GMM_.resize (K_);
128 background_GMM_.resize (K_);
133 if (image_->isOrganized ())
135 computeBetaOrganized ();
136 computeNLinksOrganized ();
140 computeBetaNonOrganized ();
141 computeNLinksNonOrganized ();
144 initialized_ =
false;
148 template <
typename Po
intT>
void 151 graph_.addEdge (v1, v2, capacity, rev_capacity);
154 template <
typename Po
intT>
void 157 graph_.addSourceEdge (v, source_capacity);
158 graph_.addTargetEdge (v, sink_capacity);
161 template <
typename Po
intT>
void 170 for (
const int &index : indices->indices)
183 template <
typename Po
intT>
void 187 buildGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_);
193 template <
typename Po
intT>
int 197 learnGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_);
202 float flow = graph_.solve ();
204 int changed = updateHardSegmentation ();
205 PCL_INFO (
"%d pixels changed segmentation (max flow = %f)\n", changed, flow);
210 template <
typename Po
intT>
void 213 std::size_t changed = indices_->size ();
216 changed = refineOnce ();
219 template <
typename Po
intT>
int 226 const int number_of_indices =
static_cast<int> (indices_->size ());
227 for (
int i_point = 0; i_point < number_of_indices; ++i_point)
238 if (isSource (graph_nodes_[i_point]))
244 if (old_value != hard_segmentation_ [i_point])
250 template <
typename Po
intT>
void 254 for (
const int &index : indices->indices)
259 for (
const int &index : indices->indices)
263 for (
const int &index : indices->indices)
267 template <
typename Po
intT>
void 271 const int number_of_indices =
static_cast<int> (indices_->size ());
274 graph_nodes_.clear ();
275 graph_nodes_.resize (indices_->size ());
276 int start = graph_.addNodes (indices_->size ());
277 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
279 graph_nodes_[idx] = start;
284 for (
int i_point = 0; i_point < number_of_indices; ++i_point)
286 int point_index = (*indices_) [i_point];
289 switch (trimap_[point_index])
293 fore =
static_cast<float> (-std::log (background_GMM_.probabilityDensity ((*image_)[point_index])));
294 back =
static_cast<float> (-std::log (foreground_GMM_.probabilityDensity ((*image_)[point_index])));
310 setTerminalWeights (graph_nodes_[i_point], fore, back);
314 for (
int i_point = 0; i_point < number_of_indices; ++i_point)
316 const NLinks &n_link = n_links_[i_point];
319 int point_index = (*indices_) [i_point];
320 std::vector<float>::const_iterator weights_it = n_link.
weights.begin ();
321 for (
auto indices_it = n_link.
indices.cbegin (); indices_it != n_link.
indices.cend (); ++indices_it, ++weights_it)
323 if ((*indices_it != point_index) && (*indices_it > -1))
325 addEdge (graph_nodes_[i_point], graph_nodes_[*indices_it], *weights_it, *weights_it);
332 template <
typename Po
intT>
void 335 const int number_of_indices =
static_cast<int> (indices_->size ());
336 for (
int i_point = 0; i_point < number_of_indices; ++i_point)
338 NLinks &n_link = n_links_[i_point];
341 int point_index = (*indices_) [i_point];
342 auto dists_it = n_link.
dists.cbegin ();
343 auto weights_it = n_link.
weights.begin ();
344 for (
auto indices_it = n_link.
indices.cbegin (); indices_it != n_link.
indices.cend (); ++indices_it, ++dists_it, ++weights_it)
346 if (*indices_it != point_index)
349 float color_distance = *weights_it;
351 *weights_it =
static_cast<float> (lambda_ * std::exp (-beta_ * color_distance) / sqrt (*dists_it));
358 template <
typename Po
intT>
void 361 for(
unsigned int y = 0; y < image_->height; ++y )
363 for(
unsigned int x = 0; x < image_->width; ++x )
367 std::size_t point_index = y * input_->width + x;
368 NLinks &links = n_links_[point_index];
370 if( x > 0 && y < image_->height-1 )
373 if( y < image_->height-1 )
376 if( x < image_->width-1 && y < image_->height-1 )
379 if( x < image_->width-1 )
385 template <
typename Po
intT>
void 389 std::size_t edges = 0;
391 const int number_of_indices =
static_cast<int> (indices_->size ());
393 for (
int i_point = 0; i_point < number_of_indices; i_point++)
395 int point_index = (*indices_)[i_point];
396 const PointT& point = input_->points [point_index];
399 NLinks &links = n_links_[i_point];
400 int found = tree_->nearestKSearch (point, nb_neighbours_, links.
indices, links.
dists);
405 for (std::vector<int>::const_iterator nn_it = links.
indices.begin (); nn_it != links.
indices.end (); ++nn_it)
407 if (*nn_it != point_index)
410 links.
weights.push_back (color_distance);
411 result+= color_distance;
421 beta_ = 1e5 / (2*result / edges);
424 template <
typename Po
intT>
void 428 std::size_t edges = 0;
430 for (
unsigned int y = 0; y < input_->height; ++y)
432 for (
unsigned int x = 0; x < input_->width; ++x)
434 std::size_t point_index = y * input_->width + x;
435 NLinks &links = n_links_[point_index];
441 if (x > 0 && y < input_->height-1)
443 std::size_t upleft = (y+1) * input_->width + x - 1;
445 links.
dists[0] = std::sqrt (2.f);
453 if (y < input_->height-1)
455 std::size_t up = (y+1) * input_->width + x;
465 if (x < input_->width-1 && y < input_->height-1)
467 std::size_t upright = (y+1) * input_->width + x + 1;
469 links.
dists[2] = std::sqrt (2.f);
471 image_->points [upright]);
477 if (x < input_->width-1)
479 std::size_t right = y * input_->width + x + 1;
491 beta_ = 1e5 / (2*result / edges);
494 template <
typename Po
intT>
void 500 template <
typename Po
intT>
void 506 clusters[0].indices.reserve (indices_->size ());
507 clusters[1].indices.reserve (indices_->size ());
509 assert (hard_segmentation_.size () == indices_->size ());
510 const int indices_size =
static_cast<int> (indices_->size ());
511 for (
int i = 0; i < indices_size; ++i)
513 clusters[1].indices.push_back (i);
515 clusters[0].indices.push_back (i);
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
virtual void fitGMMs()
Fit Gaussian Multi Models.
void setTrimap(const PointIndicesConstPtr &indices, segmentation::grabcut::TrimapValue t)
Edit Trimap.
void computeNLinksOrganized()
Compute NLinks from image.
void initGraph()
Build the graph for GraphCut.
void addEdge(vertex_descriptor v1, vertex_descriptor v2, float capacity, float rev_capacity)
Add an edge to the graph, graph must be oriented so we add the edge and its reverse.
void computeBetaNonOrganized()
Compute beta from cloud.
void extract(std::vector< pcl::PointIndices > &clusters)
This method launches the segmentation algorithm and returns the clusters that were obtained during th...
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
Define standard C methods to do distance calculations.
std::vector< int > indices
std::vector< float > weights
SegmentationValue
Grabcut derived hard segmentation values.
void computeBetaOrganized()
Compute beta from image.
PCL_EXPORTS void learnGMMs(const Image &image, const std::vector< int > &indices, const std::vector< SegmentationValue > &hard_segmentation, std::vector< std::size_t > &components, GMM &background_GMM, GMM &foreground_GMM)
Iteratively learn GMMs using GrabCut updating algorithm.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
std::vector< float > dists
void computeL()
Compute L parameter from given lambda.
PCL_EXPORTS void buildGMMs(const Image &image, const std::vector< int > &indices, const std::vector< SegmentationValue > &hardSegmentation, std::vector< std::size_t > &components, GMM &background_GMM, GMM &foreground_GMM)
Build the initial GMMs using the Orchard and Bouman color clustering algorithm.
Structure to save RGB colors into floats.
A point structure representing Euclidean xyz coordinates, and the RGB color.
int updateHardSegmentation()
TrimapValue
User supplied Trimap values.
void setTerminalWeights(vertex_descriptor v, float source_capacity, float sink_capacity)
Set the weights of SOURCE –> v and v –> SINK.
virtual void refine()
Run Grabcut refinement on the hard segmentation.
PointIndices::ConstPtr PointIndicesConstPtr
void computeNLinksNonOrganized()
Compute NLinks from cloud.
typename PointCloud::ConstPtr PointCloudConstPtr
void setBackgroundPointsIndices(int x1, int y1, int x2, int y2)
Set background indices, foreground indices = indices \ background indices.
pcl::segmentation::grabcut::BoykovKolmogorov::vertex_descriptor vertex_descriptor