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));
60template <
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;
68template <
typename Po
intT>
69grabcut::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);
80template <
typename Po
intT>
void
86template <
typename Po
intT>
bool
92 PCL_ERROR (
"[pcl::GrabCut::initCompute ()] Init failed!\n");
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_);
121 trimap_ = std::vector<segmentation::grabcut::TrimapValue> (
indices_size, TrimapUnknown);
122 hard_segmentation_ = std::vector<segmentation::grabcut::SegmentationValue> (
indices_size,
123 SegmentationBackground);
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;
149template <
typename Po
intT>
void
155template <
typename Po
intT>
void
162template <
typename Po
intT>
void
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)
173 trimap_[index] = TrimapUnknown;
174 hard_segmentation_[index] = SegmentationForeground;
184template <
typename Po
intT>
void
188 buildGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_);
194template <
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);
211template <
typename Po
intT>
void
220template <
typename Po
intT>
int
232 if (trimap_ [
i_point] == TrimapBackground)
233 hard_segmentation_ [
i_point] = SegmentationBackground;
235 if (trimap_ [
i_point] == TrimapForeground)
236 hard_segmentation_ [
i_point] = SegmentationForeground;
239 if (isSource (graph_nodes_[
i_point]))
240 hard_segmentation_ [
i_point] = SegmentationForeground;
242 hard_segmentation_ [
i_point] = SegmentationBackground;
251template <
typename Po
intT>
void
255 for (
const auto &index : indices->indices)
259 if (
t == TrimapForeground)
260 for (
const auto &index : indices->indices)
261 hard_segmentation_[index] = SegmentationForeground;
263 if (
t == TrimapBackground)
264 for (
const auto &index : indices->indices)
265 hard_segmentation_[index] = SegmentationBackground;
268template <
typename Po
intT>
void
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;
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])));
298 case TrimapBackground :
311 setTerminalWeights (graph_nodes_[
i_point],
fore, back);
333template <
typename Po
intT>
void
359template <
typename Po
intT>
void
362 for(
unsigned int y = 0; y < image_->height; ++y )
364 for(
unsigned int x = 0; x < image_->width; ++x )
372 links.weights[0] = lambda_ * std::exp (-beta_ *
links.weights[0]) /
links.dists[0];
375 links.weights[1] = lambda_ * std::exp (-beta_ *
links.weights[1]) /
links.dists[1];
378 links.weights[2] = lambda_ * std::exp (-beta_ *
links.weights[2]) /
links.dists[2];
381 links.weights[3] = lambda_ * std::exp (-beta_ *
links.weights[3]) /
links.dists[3];
386template <
typename Po
intT>
void
390 std::size_t edges = 0;
401 int found = tree_->nearestKSearch (point, nb_neighbours_,
links.indices,
links.dists);
416 links.weights.push_back (0.f);
425template <
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)
444 std::size_t
upleft = (y+1) * input_->width + x - 1;
446 links.dists[0] = std::sqrt (2.f);
456 std::size_t
up = (y+1) * input_->width + x;
468 std::size_t
upright = (y+1) * input_->width + x + 1;
470 links.dists[2] = std::sqrt (2.f);
480 std::size_t right = y * input_->width + x + 1;
481 links.indices[3] = right;
495template <
typename Po
intT>
void
501template <
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 ());
513 if (hard_segmentation_[i] == SegmentationForeground)
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
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.
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.
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.
PointIndices::ConstPtr PointIndicesConstPtr
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Define standard C methods to do distance calculations.
TrimapValue
User supplied Trimap values.
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...
static constexpr index_t UNAVAILABLE
A point structure representing Euclidean xyz coordinates, and the RGB color.
Structure to save RGB colors into floats.