38 #ifndef PCL_SEGMENTATION_IMPL_GRABCUT_HPP
39 #define PCL_SEGMENTATION_IMPL_GRABCUT_HPP
41 #include <pcl/search/organized.h>
42 #include <pcl/search/kdtree.h>
51 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));
55 template <
typename Po
intT>
58 r =
static_cast<float> (p.r) / 255.0;
59 g =
static_cast<float> (p.g) / 255.0;
60 b =
static_cast<float> (p.b) / 255.0;
63 template <
typename Po
intT>
64 pcl::segmentation::grabcut::Color::operator
PointT ()
const
67 p.r =
static_cast<std::uint32_t
> (r * 255);
68 p.g =
static_cast<std::uint32_t
> (g * 255);
69 p.b =
static_cast<std::uint32_t
> (b * 255);
73 template <
typename Po
intT>
void
79 template <
typename Po
intT>
bool
85 PCL_ERROR (
"[pcl::GrabCut::initCompute ()] Init failed!");
89 std::vector<pcl::PCLPointField> in_fields_;
90 if ((pcl::getFieldIndex<PointT> (
"rgb", in_fields_) == -1) &&
91 (pcl::getFieldIndex<PointT> (
"rgba", in_fields_) == -1))
93 PCL_ERROR (
"[pcl::GrabCut::initCompute ()] No RGB data available, aborting!");
98 image_.reset (
new Image (input_->width, input_->height));
99 for (std::size_t i = 0; i < input_->size (); ++i)
101 (*image_) [i] =
Color (input_->points[i]);
103 width_ = image_->width;
104 height_ = image_->height;
107 if (!tree_ && !input_->isOrganized ())
110 tree_->setInputCloud (input_);
113 const std::size_t indices_size = indices_->size ();
114 trimap_ = std::vector<segmentation::grabcut::TrimapValue> (indices_size,
TrimapUnknown);
115 hard_segmentation_ = std::vector<segmentation::grabcut::SegmentationValue> (indices_size,
117 GMM_component_.resize (indices_size);
118 n_links_.resize (indices_size);
121 foreground_GMM_.resize (K_);
122 background_GMM_.resize (K_);
127 if (image_->isOrganized ())
129 computeBetaOrganized ();
130 computeNLinksOrganized ();
134 computeBetaNonOrganized ();
135 computeNLinksNonOrganized ();
138 initialized_ =
false;
142 template <
typename Po
intT>
void
145 graph_.addEdge (v1, v2, capacity, rev_capacity);
148 template <
typename Po
intT>
void
151 graph_.addSourceEdge (v, source_capacity);
152 graph_.addTargetEdge (v, sink_capacity);
155 template <
typename Po
intT>
void
164 for (
const int &index : indices->indices)
177 template <
typename Po
intT>
void
181 buildGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_);
187 template <
typename Po
intT>
int
191 learnGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_);
196 float flow = graph_.solve ();
198 int changed = updateHardSegmentation ();
199 PCL_INFO (
"%d pixels changed segmentation (max flow = %f)\n", changed, flow);
204 template <
typename Po
intT>
void
207 std::size_t changed = indices_->size ();
210 changed = refineOnce ();
213 template <
typename Po
intT>
int
220 const int number_of_indices =
static_cast<int> (indices_->size ());
221 for (
int i_point = 0; i_point < number_of_indices; ++i_point)
232 if (isSource (graph_nodes_[i_point]))
238 if (old_value != hard_segmentation_ [i_point])
244 template <
typename Po
intT>
void
248 for (
const int &index : indices->indices)
253 for (
const int &index : indices->indices)
257 for (
const int &index : indices->indices)
261 template <
typename Po
intT>
void
265 const int number_of_indices =
static_cast<int> (indices_->size ());
268 graph_nodes_.clear ();
269 graph_nodes_.resize (indices_->size ());
270 int start = graph_.addNodes (indices_->size ());
271 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
273 graph_nodes_[idx] = start;
278 for (
int i_point = 0; i_point < number_of_indices; ++i_point)
280 int point_index = (*indices_) [i_point];
283 switch (trimap_[point_index])
287 fore =
static_cast<float> (-std::log (background_GMM_.probabilityDensity (image_->points[point_index])));
288 back =
static_cast<float> (-std::log (foreground_GMM_.probabilityDensity (image_->points[point_index])));
304 setTerminalWeights (graph_nodes_[i_point], fore, back);
308 for (
int i_point = 0; i_point < number_of_indices; ++i_point)
310 const NLinks &n_link = n_links_[i_point];
313 int point_index = (*indices_) [i_point];
314 std::vector<float>::const_iterator weights_it = n_link.
weights.begin ();
315 for (
auto indices_it = n_link.
indices.cbegin (); indices_it != n_link.
indices.cend (); ++indices_it, ++weights_it)
317 if ((*indices_it != point_index) && (*indices_it > -1))
319 addEdge (graph_nodes_[i_point], graph_nodes_[*indices_it], *weights_it, *weights_it);
326 template <
typename Po
intT>
void
329 const int number_of_indices =
static_cast<int> (indices_->size ());
330 for (
int i_point = 0; i_point < number_of_indices; ++i_point)
332 NLinks &n_link = n_links_[i_point];
335 int point_index = (*indices_) [i_point];
336 auto dists_it = n_link.
dists.cbegin ();
337 auto weights_it = n_link.
weights.begin ();
338 for (
auto indices_it = n_link.
indices.cbegin (); indices_it != n_link.
indices.cend (); ++indices_it, ++dists_it, ++weights_it)
340 if (*indices_it != point_index)
343 float color_distance = *weights_it;
345 *weights_it =
static_cast<float> (lambda_ * std::exp (-beta_ * color_distance) / sqrt (*dists_it));
352 template <
typename Po
intT>
void
355 for(
unsigned int y = 0; y < image_->height; ++y )
357 for(
unsigned int x = 0; x < image_->width; ++x )
361 std::size_t point_index = y * input_->width + x;
362 NLinks &links = n_links_[point_index];
364 if( x > 0 && y < image_->height-1 )
367 if( y < image_->height-1 )
370 if( x < image_->width-1 && y < image_->height-1 )
373 if( x < image_->width-1 )
379 template <
typename Po
intT>
void
383 std::size_t edges = 0;
385 const int number_of_indices =
static_cast<int> (indices_->size ());
387 for (
int i_point = 0; i_point < number_of_indices; i_point++)
389 int point_index = (*indices_)[i_point];
390 const PointT& point = input_->points [point_index];
393 NLinks &links = n_links_[i_point];
394 int found = tree_->nearestKSearch (point, nb_neighbours_, links.
indices, links.
dists);
399 for (std::vector<int>::const_iterator nn_it = links.
indices.begin (); nn_it != links.
indices.end (); ++nn_it)
401 if (*nn_it != point_index)
404 links.
weights.push_back (color_distance);
405 result+= color_distance;
415 beta_ = 1e5 / (2*result / edges);
418 template <
typename Po
intT>
void
422 std::size_t edges = 0;
424 for (
unsigned int y = 0; y < input_->height; ++y)
426 for (
unsigned int x = 0; x < input_->width; ++x)
428 std::size_t point_index = y * input_->width + x;
429 NLinks &links = n_links_[point_index];
435 if (x > 0 && y < input_->height-1)
437 std::size_t upleft = (y+1) * input_->width + x - 1;
439 links.
dists[0] = std::sqrt (2.f);
441 image_->points[upleft]);
447 if (y < input_->height-1)
449 std::size_t up = (y+1) * input_->width + x;
459 if (x < input_->width-1 && y < input_->height-1)
461 std::size_t upright = (y+1) * input_->width + x + 1;
463 links.
dists[2] = std::sqrt (2.f);
465 image_->points [upright]);
471 if (x < input_->width-1)
473 std::size_t right = y * input_->width + x + 1;
477 image_->points[right]);
485 beta_ = 1e5 / (2*result / edges);
488 template <
typename Po
intT>
void
494 template <
typename Po
intT>
void
500 clusters[0].indices.reserve (indices_->size ());
501 clusters[1].indices.reserve (indices_->size ());
503 assert (hard_segmentation_.size () == indices_->size ());
504 const int indices_size =
static_cast<int> (indices_->size ());
505 for (
int i = 0; i < indices_size; ++i)
507 clusters[1].indices.push_back (i);
509 clusters[0].indices.push_back (i);