Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
grabcut_segmentation.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2012-, Open Perception, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of Willow Garage, Inc. nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 */
37
38#pragma once
39
41#include <pcl/common/io.h> // for getFieldIndex
42#include <pcl/common/point_tests.h> // for pcl::isFinite
43#include <pcl/search/organized.h>
44#include <pcl/search/kdtree.h>
45
46
47namespace pcl
48{
49
50template <>
53{
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));
55}
56
57namespace segmentation
58{
59
60template <typename PointT>
62{
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;
66}
67
68template <typename PointT>
69grabcut::Color::operator PointT () const
70{
71 PointT p;
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);
75 return (p);
76}
77
78} // namespace segmentation
79
80template <typename PointT> void
82{
83 input_ = cloud;
84}
85
86template <typename PointT> bool
88{
89 using namespace pcl::segmentation::grabcut;
91 {
92 PCL_ERROR ("[pcl::GrabCut::initCompute ()] Init failed!\n");
93 return (false);
94 }
95
96 std::vector<pcl::PCLPointField> in_fields_;
97 if ((pcl::getFieldIndex<PointT> ("rgb", in_fields_) == -1) &&
98 (pcl::getFieldIndex<PointT> ("rgba", in_fields_) == -1))
99 {
100 PCL_ERROR ("[pcl::GrabCut::initCompute ()] No RGB data available, aborting!\n");
101 return (false);
102 }
103
104 // Initialize the working image
105 image_.reset (new Image (input_->width, input_->height));
106 for (std::size_t i = 0; i < input_->size (); ++i)
107 {
108 (*image_) [i] = Color ((*input_)[i]);
109 }
110 width_ = image_->width;
111 height_ = image_->height;
112
113 // Initialize the spatial locator
114 if (!tree_ && !input_->isOrganized ())
115 {
116 tree_.reset (new pcl::search::KdTree<PointT> (true));
117 tree_->setInputCloud (input_);
118 }
119
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,
123 SegmentationBackground);
124 GMM_component_.resize (indices_size);
125 n_links_.resize (indices_size);
126
127 // soft_segmentation_ = 0; // Not yet implemented
128 foreground_GMM_.resize (K_);
129 background_GMM_.resize (K_);
130
131 //set some constants
132 computeL ();
133
134 if (image_->isOrganized ())
135 {
136 computeBetaOrganized ();
137 computeNLinksOrganized ();
138 }
139 else
140 {
141 computeBetaNonOrganized ();
142 computeNLinksNonOrganized ();
143 }
144
145 initialized_ = false;
146 return (true);
147}
148
149template <typename PointT> void
150GrabCut<PointT>::addEdge (vertex_descriptor v1, vertex_descriptor v2, float capacity, float rev_capacity)
151{
152 graph_.addEdge (v1, v2, capacity, rev_capacity);
153}
154
155template <typename PointT> void
156GrabCut<PointT>::setTerminalWeights (vertex_descriptor v, float source_capacity, float sink_capacity)
157{
158 graph_.addSourceEdge (v, source_capacity);
159 graph_.addTargetEdge (v, sink_capacity);
160}
161
162template <typename PointT> void
164{
165 using namespace pcl::segmentation::grabcut;
166 if (!initCompute ())
167 return;
168
169 std::fill (trimap_.begin (), trimap_.end (), TrimapBackground);
170 std::fill (hard_segmentation_.begin (), hard_segmentation_.end (), SegmentationBackground);
171 for (const auto &index : indices->indices)
172 {
173 trimap_[index] = TrimapUnknown;
174 hard_segmentation_[index] = SegmentationForeground;
175 }
176
177 if (!initialized_)
178 {
179 fitGMMs ();
180 initialized_ = true;
181 }
182}
183
184template <typename PointT> void
186{
187 // Step 3: Build GMMs using Orchard-Bouman clustering algorithm
188 buildGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_);
189
190 // Initialize the graph for graphcut (do this here so that the T-Link debugging image will be initialized)
191 initGraph ();
192}
193
194template <typename PointT> int
196{
197 // Steps 4 and 5: Learn new GMMs from current segmentation
198 learnGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_);
199
200 // Step 6: Run GraphCut and update segmentation
201 initGraph ();
202
203 float flow = graph_.solve ();
204
205 int changed = updateHardSegmentation ();
206 PCL_INFO ("%d pixels changed segmentation (max flow = %f)\n", changed, flow);
207
208 return (changed);
209}
210
211template <typename PointT> void
213{
214 std::size_t changed = indices_->size ();
215
216 while (changed)
217 changed = refineOnce ();
218}
219
220template <typename PointT> int
222{
223 using namespace pcl::segmentation::grabcut;
224
225 int changed = 0;
226
227 const int number_of_indices = static_cast<int> (indices_->size ());
228 for (int i_point = 0; i_point < number_of_indices; ++i_point)
229 {
230 SegmentationValue old_value = hard_segmentation_ [i_point];
231
232 if (trimap_ [i_point] == TrimapBackground)
233 hard_segmentation_ [i_point] = SegmentationBackground;
234 else
235 if (trimap_ [i_point] == TrimapForeground)
236 hard_segmentation_ [i_point] = SegmentationForeground;
237 else // TrimapUnknown
238 {
239 if (isSource (graph_nodes_[i_point]))
240 hard_segmentation_ [i_point] = SegmentationForeground;
241 else
242 hard_segmentation_ [i_point] = SegmentationBackground;
243 }
244
245 if (old_value != hard_segmentation_ [i_point])
246 ++changed;
247 }
248 return (changed);
249}
250
251template <typename PointT> void
253{
254 using namespace pcl::segmentation::grabcut;
255 for (const auto &index : indices->indices)
256 trimap_[index] = t;
257
258 // Immediately set the hard segmentation as well so that the display will update.
259 if (t == TrimapForeground)
260 for (const auto &index : indices->indices)
261 hard_segmentation_[index] = SegmentationForeground;
262 else
263 if (t == TrimapBackground)
264 for (const auto &index : indices->indices)
265 hard_segmentation_[index] = SegmentationBackground;
266}
267
268template <typename PointT> void
270{
271 using namespace pcl::segmentation::grabcut;
272 const int number_of_indices = static_cast<int> (indices_->size ());
273 // Set up the graph (it can only be used once, so we have to recreate it each time the graph is updated)
274 graph_.clear ();
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)
279 {
280 graph_nodes_[idx] = start;
281 ++start;
282 }
283
284 // Set T-Link weights
285 for (int i_point = 0; i_point < number_of_indices; ++i_point)
286 {
287 int point_index = (*indices_) [i_point];
288 float back, fore;
289
290 switch (trimap_[point_index])
291 {
292 case TrimapUnknown :
293 {
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])));
296 break;
297 }
298 case TrimapBackground :
299 {
300 fore = 0;
301 back = L_;
302 break;
303 }
304 default :
305 {
306 fore = L_;
307 back = 0;
308 }
309 }
310
311 setTerminalWeights (graph_nodes_[i_point], fore, back);
312 }
313
314 // Set N-Link weights from precomputed values
315 for (int i_point = 0; i_point < number_of_indices; ++i_point)
316 {
317 const NLinks &n_link = n_links_[i_point];
318 if (n_link.nb_links > 0)
319 {
320 const auto 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)
323 {
324 if ((*indices_it != point_index) && (*indices_it != UNAVAILABLE))
325 {
326 addEdge (graph_nodes_[i_point], graph_nodes_[*indices_it], *weights_it, *weights_it);
327 }
328 }
329 }
330 }
331}
332
333template <typename PointT> void
335{
336 const int number_of_indices = static_cast<int> (indices_->size ());
337 for (int i_point = 0; i_point < number_of_indices; ++i_point)
338 {
339 NLinks &n_link = n_links_[i_point];
340 if (n_link.nb_links > 0)
341 {
342 const auto 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)
346 {
347 if (*indices_it != point_index)
348 {
349 // We saved the color distance previously at the computeBeta stage for optimization purpose
350 float color_distance = *weights_it;
351 // Set the real weight
352 *weights_it = static_cast<float> (lambda_ * std::exp (-beta_ * color_distance) / sqrt (*dists_it));
353 }
354 }
355 }
356 }
357}
358
359template <typename PointT> void
361{
362 for( unsigned int y = 0; y < image_->height; ++y )
363 {
364 for( unsigned int x = 0; x < image_->width; ++x )
365 {
366 // We saved the color and euclidean distance previously at the computeBeta stage for
367 // optimization purpose but here we compute the real weight
368 std::size_t point_index = y * input_->width + x;
369 NLinks &links = n_links_[point_index];
370
371 if( x > 0 && y < image_->height-1 )
372 links.weights[0] = lambda_ * std::exp (-beta_ * links.weights[0]) / links.dists[0];
373
374 if( y < image_->height-1 )
375 links.weights[1] = lambda_ * std::exp (-beta_ * links.weights[1]) / links.dists[1];
376
377 if( x < image_->width-1 && y < image_->height-1 )
378 links.weights[2] = lambda_ * std::exp (-beta_ * links.weights[2]) / links.dists[2];
379
380 if( x < image_->width-1 )
381 links.weights[3] = lambda_ * std::exp (-beta_ * links.weights[3]) / links.dists[3];
382 }
383 }
384}
385
386template <typename PointT> void
388{
389 float result = 0;
390 std::size_t edges = 0;
391
392 const int number_of_indices = static_cast<int> (indices_->size ());
393
394 for (int i_point = 0; i_point < number_of_indices; i_point++)
395 {
396 const auto point_index = (*indices_)[i_point];
397 const PointT& point = input_->points [point_index];
398 if (pcl::isFinite (point))
399 {
400 NLinks &links = n_links_[i_point];
401 int found = tree_->nearestKSearch (point, nb_neighbours_, links.indices, links.dists);
402 if (found > 1)
403 {
404 links.nb_links = found - 1;
405 links.weights.reserve (links.nb_links);
406 for (const auto& nn_index : links.indices)
407 {
408 if (nn_index != point_index)
409 {
410 float color_distance = squaredEuclideanDistance ((*image_)[point_index], (*image_)[nn_index]);
411 links.weights.push_back (color_distance);
412 result+= color_distance;
413 ++edges;
414 }
415 else
416 links.weights.push_back (0.f);
417 }
418 }
419 }
420 }
421
422 beta_ = 1e5 / (2*result / edges);
423}
424
425template <typename PointT> void
427{
428 float result = 0;
429 std::size_t edges = 0;
430
431 for (unsigned int y = 0; y < input_->height; ++y)
432 {
433 for (unsigned int x = 0; x < input_->width; ++x)
434 {
435 std::size_t point_index = y * input_->width + x;
436 NLinks &links = n_links_[point_index];
437 links.nb_links = 4;
438 links.weights.resize (links.nb_links, 0);
439 links.dists.resize (links.nb_links, 0);
440 links.indices.resize (links.nb_links, -1);
441
442 if (x > 0 && y < input_->height-1)
443 {
444 std::size_t upleft = (y+1) * input_->width + x - 1;
445 links.indices[0] = upleft;
446 links.dists[0] = std::sqrt (2.f);
447 float color_dist = squaredEuclideanDistance ((*image_)[point_index],
448 (*image_)[upleft]);
449 links.weights[0] = color_dist;
450 result+= color_dist;
451 edges++;
452 }
453
454 if (y < input_->height-1)
455 {
456 std::size_t up = (y+1) * input_->width + x;
457 links.indices[1] = up;
458 links.dists[1] = 1;
459 float color_dist = squaredEuclideanDistance ((*image_)[point_index],
460 (*image_)[up]);
461 links.weights[1] = color_dist;
462 result+= color_dist;
463 edges++;
464 }
465
466 if (x < input_->width-1 && y < input_->height-1)
467 {
468 std::size_t upright = (y+1) * input_->width + x + 1;
469 links.indices[2] = upright;
470 links.dists[2] = std::sqrt (2.f);
471 float color_dist = squaredEuclideanDistance ((*image_)[point_index],
472 image_->points [upright]);
473 links.weights[2] = color_dist;
474 result+= color_dist;
475 edges++;
476 }
477
478 if (x < input_->width-1)
479 {
480 std::size_t right = y * input_->width + x + 1;
481 links.indices[3] = right;
482 links.dists[3] = 1;
483 float color_dist = squaredEuclideanDistance ((*image_)[point_index],
484 (*image_)[right]);
485 links.weights[3] = color_dist;
486 result+= color_dist;
487 edges++;
488 }
489 }
490 }
491
492 beta_ = 1e5 / (2*result / edges);
493}
494
495template <typename PointT> void
497{
498 L_ = 8*lambda_ + 1;
499}
500
501template <typename PointT> void
502GrabCut<PointT>::extract (std::vector<pcl::PointIndices>& clusters)
503{
504 using namespace pcl::segmentation::grabcut;
505 clusters.clear ();
506 clusters.resize (2);
507 clusters[0].indices.reserve (indices_->size ());
508 clusters[1].indices.reserve (indices_->size ());
509 refine ();
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)
513 if (hard_segmentation_[i] == SegmentationForeground)
514 clusters[1].indices.push_back (i);
515 else
516 clusters[0].indices.push_back (i);
517}
518
519} // namespace pcl
520
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.
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.
typename PCLBase< PointT >::PointCloudConstPtr PointCloudConstPtr
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.
virtual int refineOnce()
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.
PCL base class.
Definition pcl_base.h:70
PointIndices::ConstPtr PointIndicesConstPtr
Definition pcl_base.h:77
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...
Definition kdtree.h:62
Define standard C methods to do distance calculations.
TrimapValue
User supplied Trimap values.
SegmentationValue
Grabcut derived hard segmentation values.
int getFieldIndex(const pcl::PointCloud< PointT > &, const std::string &field_name, std::vector< pcl::PCLPointField > &fields)
Definition io.hpp:54
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
Definition distances.h:182
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...
Definition point_tests.h:55
static constexpr index_t UNAVAILABLE
Definition pcl_base.h:62
A point structure representing Euclidean xyz coordinates, and the RGB color.
Structure to save RGB colors into floats.