41 #include <pcl/common/io.h>
42 #include <pcl/common/point_tests.h>
43 #include <pcl/search/organized.h>
44 #include <pcl/search/kdtree.h>
54 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));
57 namespace segmentation
60 template <
typename Po
intT>
63 r =
static_cast<float> (p.r) / 255.0;
64 g =
static_cast<float> (p.g) / 255.0;
65 b =
static_cast<float> (p.b) / 255.0;
68 template <
typename Po
intT>
69 grabcut::Color::operator
PointT ()
const
72 p.r =
static_cast<std::uint32_t
> (r * 255);
73 p.g =
static_cast<std::uint32_t
> (g * 255);
74 p.b =
static_cast<std::uint32_t
> (b * 255);
80 template <
typename Po
intT>
void
86 template <
typename Po
intT>
bool
92 PCL_ERROR (
"[pcl::GrabCut::initCompute ()] Init failed!\n");
96 std::vector<pcl::PCLPointField> in_fields_;
97 if ((pcl::getFieldIndex<PointT> (
"rgb", in_fields_) == -1) &&
98 (pcl::getFieldIndex<PointT> (
"rgba", in_fields_) == -1))
100 PCL_ERROR (
"[pcl::GrabCut::initCompute ()] No RGB data available, aborting!\n");
105 image_.reset (
new Image (input_->width, input_->height));
106 for (std::size_t i = 0; i < input_->size (); ++i)
108 (*image_) [i] =
Color ((*input_)[i]);
110 width_ = image_->width;
111 height_ = image_->height;
114 if (!tree_ && !input_->isOrganized ())
117 tree_->setInputCloud (input_);
120 const std::size_t indices_size = indices_->size ();
121 trimap_ = std::vector<segmentation::grabcut::TrimapValue> (indices_size,
TrimapUnknown);
122 hard_segmentation_ = std::vector<segmentation::grabcut::SegmentationValue> (indices_size,
124 GMM_component_.resize (indices_size);
125 n_links_.resize (indices_size);
128 foreground_GMM_.resize (K_);
129 background_GMM_.resize (K_);
134 if (image_->isOrganized ())
136 computeBetaOrganized ();
137 computeNLinksOrganized ();
141 computeBetaNonOrganized ();
142 computeNLinksNonOrganized ();
145 initialized_ =
false;
149 template <
typename Po
intT>
void
152 graph_.addEdge (v1, v2, capacity, rev_capacity);
155 template <
typename Po
intT>
void
158 graph_.addSourceEdge (v, source_capacity);
159 graph_.addTargetEdge (v, sink_capacity);
162 template <
typename Po
intT>
void
171 for (
const auto &index : indices->indices)
184 template <
typename Po
intT>
void
188 buildGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_);
194 template <
typename Po
intT>
int
198 learnGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_);
203 float flow = graph_.solve ();
205 int changed = updateHardSegmentation ();
206 PCL_INFO (
"%d pixels changed segmentation (max flow = %f)\n", changed, flow);
211 template <
typename Po
intT>
void
214 std::size_t changed = indices_->size ();
217 changed = refineOnce ();
220 template <
typename Po
intT>
int
227 const int number_of_indices =
static_cast<int> (indices_->size ());
228 for (
int i_point = 0; i_point < number_of_indices; ++i_point)
239 if (isSource (graph_nodes_[i_point]))
245 if (old_value != hard_segmentation_ [i_point])
251 template <
typename Po
intT>
void
255 for (
const auto &index : indices->indices)
260 for (
const auto &index : indices->indices)
264 for (
const auto &index : indices->indices)
268 template <
typename Po
intT>
void
272 const int number_of_indices =
static_cast<int> (indices_->size ());
275 graph_nodes_.clear ();
276 graph_nodes_.resize (indices_->size ());
277 int start = graph_.addNodes (indices_->size ());
278 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
280 graph_nodes_[idx] = start;
285 for (
int i_point = 0; i_point < number_of_indices; ++i_point)
287 int point_index = (*indices_) [i_point];
290 switch (trimap_[point_index])
294 fore =
static_cast<float> (-std::log (background_GMM_.probabilityDensity ((*image_)[point_index])));
295 back =
static_cast<float> (-std::log (foreground_GMM_.probabilityDensity ((*image_)[point_index])));
311 setTerminalWeights (graph_nodes_[i_point], fore, back);
315 for (
int i_point = 0; i_point < number_of_indices; ++i_point)
317 const NLinks &n_link = n_links_[i_point];
320 int point_index = (*indices_) [i_point];
321 std::vector<float>::const_iterator weights_it = n_link.
weights.begin ();
322 for (
auto indices_it = n_link.
indices.cbegin (); indices_it != n_link.
indices.cend (); ++indices_it, ++weights_it)
324 if ((*indices_it != point_index) && (*indices_it > -1))
326 addEdge (graph_nodes_[i_point], graph_nodes_[*indices_it], *weights_it, *weights_it);
333 template <
typename Po
intT>
void
336 const int number_of_indices =
static_cast<int> (indices_->size ());
337 for (
int i_point = 0; i_point < number_of_indices; ++i_point)
339 NLinks &n_link = n_links_[i_point];
342 int point_index = (*indices_) [i_point];
343 auto dists_it = n_link.
dists.cbegin ();
344 auto weights_it = n_link.
weights.begin ();
345 for (
auto indices_it = n_link.
indices.cbegin (); indices_it != n_link.
indices.cend (); ++indices_it, ++dists_it, ++weights_it)
347 if (*indices_it != point_index)
350 float color_distance = *weights_it;
352 *weights_it =
static_cast<float> (lambda_ * std::exp (-beta_ * color_distance) / sqrt (*dists_it));
359 template <
typename Po
intT>
void
362 for(
unsigned int y = 0; y < image_->height; ++y )
364 for(
unsigned int x = 0; x < image_->width; ++x )
368 std::size_t point_index = y * input_->width + x;
369 NLinks &links = n_links_[point_index];
371 if( x > 0 && y < image_->height-1 )
374 if( y < image_->height-1 )
377 if( x < image_->width-1 && y < image_->height-1 )
380 if( x < image_->width-1 )
386 template <
typename Po
intT>
void
390 std::size_t edges = 0;
392 const int number_of_indices =
static_cast<int> (indices_->size ());
394 for (
int i_point = 0; i_point < number_of_indices; i_point++)
396 int point_index = (*indices_)[i_point];
397 const PointT& point = input_->points [point_index];
400 NLinks &links = n_links_[i_point];
401 int found = tree_->nearestKSearch (point, nb_neighbours_, links.
indices, links.
dists);
406 for (
const auto& nn_index : links.
indices)
408 if (nn_index != point_index)
411 links.
weights.push_back (color_distance);
412 result+= color_distance;
422 beta_ = 1e5 / (2*result / edges);
425 template <
typename Po
intT>
void
429 std::size_t edges = 0;
431 for (
unsigned int y = 0; y < input_->height; ++y)
433 for (
unsigned int x = 0; x < input_->width; ++x)
435 std::size_t point_index = y * input_->width + x;
436 NLinks &links = n_links_[point_index];
442 if (x > 0 && y < input_->height-1)
444 std::size_t upleft = (y+1) * input_->width + x - 1;
446 links.
dists[0] = std::sqrt (2.f);
454 if (y < input_->height-1)
456 std::size_t up = (y+1) * input_->width + x;
466 if (x < input_->width-1 && y < input_->height-1)
468 std::size_t upright = (y+1) * input_->width + x + 1;
470 links.
dists[2] = std::sqrt (2.f);
472 image_->points [upright]);
478 if (x < input_->width-1)
480 std::size_t right = y * input_->width + x + 1;
492 beta_ = 1e5 / (2*result / edges);
495 template <
typename Po
intT>
void
501 template <
typename Po
intT>
void
507 clusters[0].indices.reserve (indices_->size ());
508 clusters[1].indices.reserve (indices_->size ());
510 assert (hard_segmentation_.size () == indices_->size ());
511 const int indices_size =
static_cast<int> (indices_->size ());
512 for (
int i = 0; i < indices_size; ++i)
514 clusters[1].indices.push_back (i);
516 clusters[0].indices.push_back (i);