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);
void computeL()
Compute L parameter from given lambda.
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.
virtual void fitGMMs()
Fit Gaussian Multi Models.
int updateHardSegmentation()
void computeNLinksNonOrganized()
Compute NLinks from cloud.
virtual void refine()
Run Grabcut refinement on the hard segmentation.
void setTrimap(const PointIndicesConstPtr &indices, segmentation::grabcut::TrimapValue t)
Edit Trimap.
pcl::segmentation::grabcut::BoykovKolmogorov::vertex_descriptor vertex_descriptor
void extract(std::vector< pcl::PointIndices > &clusters)
This method launches the segmentation algorithm and returns the clusters that were obtained during th...
void initGraph()
Build the graph for GraphCut.
void computeBetaOrganized()
Compute beta from image.
void computeNLinksOrganized()
Compute NLinks from image.
void setTerminalWeights(vertex_descriptor v, float source_capacity, float sink_capacity)
Set the weights of SOURCE --> v and v --> SINK.
void setBackgroundPointsIndices(int x1, int y1, int x2, int y2)
Set background indices, foreground indices = indices \ background indices.
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
void computeBetaNonOrganized()
Compute beta from cloud.
typename PointCloud::ConstPtr PointCloudConstPtr
PointIndices::ConstPtr PointIndicesConstPtr
PointCloud represents the base class in PCL for storing collections of 3D points.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Define standard C methods to do distance calculations.
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.
TrimapValue
User supplied Trimap values.
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.
SegmentationValue
Grabcut derived hard segmentation values.
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
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...
std::vector< float > weights
std::vector< float > dists
std::vector< int > indices
A point structure representing Euclidean xyz coordinates, and the RGB color.
Structure to save RGB colors into floats.