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<uint32_t
> (r * 255);
68 p.g =
static_cast<uint32_t
> (g * 255);
69 p.b =
static_cast<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> (*input_,
"rgb", in_fields_) == -1) &&
91 (pcl::getFieldIndex<PointT> (*input_,
"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 (std::vector<int>::const_iterator idx = indices->indices.begin (); idx != indices->indices.end (); ++idx)
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 std::vector<int>::const_iterator idx = indices->indices.begin ();
249 for (; idx != indices->indices.end (); ++idx)
254 for (idx = indices->indices.begin (); idx != indices->indices.end (); ++idx)
258 for (idx = indices->indices.begin (); idx != indices->indices.end (); ++idx)
262 template <
typename Po
intT>
void 266 const int number_of_indices =
static_cast<int> (indices_->size ());
269 graph_nodes_.clear ();
270 graph_nodes_.resize (indices_->size ());
271 int start = graph_.addNodes (indices_->size ());
272 for (
size_t idx = 0; idx < indices_->size (); ++idx)
274 graph_nodes_[idx] = start;
279 for (
int i_point = 0; i_point < number_of_indices; ++i_point)
281 int point_index = (*indices_) [i_point];
284 switch (trimap_[point_index])
288 fore =
static_cast<float> (-log (background_GMM_.probabilityDensity (image_->points[point_index])));
289 back =
static_cast<float> (-log (foreground_GMM_.probabilityDensity (image_->points[point_index])));
305 setTerminalWeights (graph_nodes_[i_point], fore, back);
309 for (
int i_point = 0; i_point < number_of_indices; ++i_point)
311 const NLinks &n_link = n_links_[i_point];
314 int point_index = (*indices_) [i_point];
315 std::vector<int>::const_iterator indices_it = n_link.
indices.begin ();
316 std::vector<float>::const_iterator weights_it = n_link.
weights.begin ();
317 for (; indices_it != n_link.
indices.end (); ++indices_it, ++weights_it)
319 if ((*indices_it != point_index) && (*indices_it > -1))
321 addEdge (graph_nodes_[i_point], graph_nodes_[*indices_it], *weights_it, *weights_it);
328 template <
typename Po
intT>
void 331 const int number_of_indices =
static_cast<int> (indices_->size ());
332 for (
int i_point = 0; i_point < number_of_indices; ++i_point)
334 NLinks &n_link = n_links_[i_point];
337 int point_index = (*indices_) [i_point];
338 std::vector<int>::const_iterator indices_it = n_link.
indices.begin ();
339 std::vector<float>::const_iterator dists_it = n_link.
dists.begin ();
340 std::vector<float>::iterator weights_it = n_link.
weights.begin ();
341 for (; indices_it != n_link.
indices.end (); ++indices_it, ++dists_it, ++weights_it)
343 if (*indices_it != point_index)
346 float color_distance = *weights_it;
348 *weights_it =
static_cast<float> (lambda_ * exp (-beta_ * color_distance) / sqrt (*dists_it));
355 template <
typename Po
intT>
void 358 for(
unsigned int y = 0; y < image_->height; ++y )
360 for(
unsigned int x = 0; x < image_->width; ++x )
364 std::size_t point_index = y * input_->width + x;
365 NLinks &links = n_links_[point_index];
367 if( x > 0 && y < image_->height-1 )
370 if( y < image_->height-1 )
373 if( x < image_->width-1 && y < image_->height-1 )
376 if( x < image_->width-1 )
382 template <
typename Po
intT>
void 386 std::size_t edges = 0;
388 const int number_of_indices =
static_cast<int> (indices_->size ());
390 for (
int i_point = 0; i_point < number_of_indices; i_point++)
392 int point_index = (*indices_)[i_point];
393 const PointT& point = input_->points [point_index];
396 NLinks &links = n_links_[i_point];
397 int found = tree_->nearestKSearch (point, nb_neighbours_, links.
indices, links.
dists);
402 for (std::vector<int>::const_iterator nn_it = links.
indices.begin (); nn_it != links.
indices.end (); ++nn_it)
404 if (*nn_it != point_index)
407 links.
weights.push_back (color_distance);
408 result+= color_distance;
418 beta_ = 1e5 / (2*result / edges);
421 template <
typename Po
intT>
void 425 std::size_t edges = 0;
427 for (
unsigned int y = 0; y < input_->height; ++y)
429 for (
unsigned int x = 0; x < input_->width; ++x)
431 std::size_t point_index = y * input_->width + x;
432 NLinks &links = n_links_[point_index];
438 if (x > 0 && y < input_->height-1)
440 std::size_t upleft = (y+1) * input_->width + x - 1;
442 links.
dists[0] = sqrt (2.f);
444 image_->points[upleft]);
450 if (y < input_->height-1)
452 std::size_t up = (y+1) * input_->width + x;
462 if (x < input_->width-1 && y < input_->height-1)
464 std::size_t upright = (y+1) * input_->width + x + 1;
466 links.
dists[2] = sqrt (2.f);
468 image_->points [upright]);
474 if (x < input_->width-1)
476 std::size_t right = y * input_->width + x + 1;
480 image_->points[right]);
488 beta_ = 1e5 / (2*result / edges);
491 template <
typename Po
intT>
void 497 template <
typename Po
intT>
void 503 clusters[0].indices.reserve (indices_->size ());
504 clusters[1].indices.reserve (indices_->size ());
506 assert (hard_segmentation_.size () == indices_->size ());
507 const int indices_size =
static_cast<int> (indices_->size ());
508 for (
int i = 0; i < indices_size; ++i)
510 clusters[1].indices.push_back (i);
512 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.
PointCloud::ConstPtr PointCloudConstPtr
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.
void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
std::vector< int > indices
std::vector< float > weights
SegmentationValue
Grabcut derived hard segementation values.
boost::shared_ptr< PointIndices const > PointIndicesConstPtr
void computeBetaOrganized()
Compute beta from image.
pcl::segmentation::grabcut::BoykovKolmogorov::vertex_descriptor vertex_descriptor
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. ...
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.
void computeNLinksNonOrganized()
Compute NLinks from cloud.
void setBackgroundPointsIndices(int x1, int y1, int x2, int y2)
Set background indices, foreground indices = indices \ background indices.