5 #ifndef PCL_SEGMENTATION_IMPL_GRABCUT_HPP
6 #define PCL_SEGMENTATION_IMPL_GRABCUT_HPP
8 #include <pcl/search/organized.h>
9 #include <pcl/search/kdtree.h>
10 #include <pcl/common/distances.h>
18 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));
22 template <
typename Po
intT>
void
28 template <
typename Po
intT>
bool
31 using namespace pcl::segmentation::grabcut;
34 PCL_ERROR (
"[pcl::GrabCut::initCompute ()] Init failed!");
38 if (!input_->isOrganized ())
40 PCL_ERROR (
"[pcl::GrabCut::initCompute ()] Need an organized point cloud to proceed!");
44 std::vector<pcl::PCLPointField> in_fields_;
45 if ((pcl::getFieldIndex<PointT> (*input_,
"rgb", in_fields_) == -1) &&
46 (pcl::getFieldIndex<PointT> (*input_,
"rgba", in_fields_) == -1))
48 PCL_ERROR (
"[pcl::GrabCut::initCompute ()] No RGB data available, aborting!");
53 image_.reset (
new Image (input_->width, input_->height));
54 for (std::size_t i = 0; i < input_->size (); ++i)
56 (*image_) [i] =
Color (input_->points[i]);
58 width_ = image_->width;
59 height_ = image_->height;
64 if (input_->isOrganized ())
68 tree_->setInputCloud (input_);
70 const std::size_t indices_size = indices_->size ();
71 trimap_ = std::vector<segmentation::grabcut::TrimapValue> (indices_size,
TrimapUnknown);
72 hard_segmentation_ = std::vector<segmentation::grabcut::SegmentationValue> (indices_size,
74 GMM_component_.resize (indices_size);
75 n_links_.resize (indices_size);
77 foreground_GMM_.resize (K_);
78 background_GMM_.resize (K_);
88 template <
typename Po
intT>
void
91 graph_.addEdge (v1, v2, capacity, rev_capacity);
94 template <
typename Po
intT>
void
97 graph_.addSourceEdge (v, source_capacity);
98 graph_.addTargetEdge (v, sink_capacity);
120 template <
typename Po
intT>
void
123 using namespace pcl::segmentation::grabcut;
129 for (std::vector<int>::const_iterator idx = indices->indices.begin (); idx != indices->indices.end (); ++idx)
142 template <
typename Po
intT>
void
146 buildGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_);
152 template <
typename Po
intT>
int
156 learnGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_);
161 float flow = graph_.solve ();
163 int changed = updateHardSegmentation ();
164 PCL_INFO (
"%d pixels changed segmentation (max flow = %f)\n", changed, flow);
169 template <
typename Po
intT>
void
172 std::size_t changed = indices_->size ();
175 changed = refineOnce ();
178 template <
typename Po
intT>
int
181 using namespace pcl::segmentation::grabcut;
185 const int number_of_indices =
static_cast<int> (indices_->size ());
186 for (
int i_point = 0; i_point < number_of_indices; ++i_point)
197 if (isSource (graph_nodes_[i_point]))
203 if (old_value != hard_segmentation_ [i_point])
209 template <
typename Po
intT>
void
212 using namespace pcl::segmentation::grabcut;
213 std::vector<int>::const_iterator idx = indices->indices.begin ();
214 for (; idx != indices->indices.end (); ++idx)
219 for (idx = indices->indices.begin (); idx != indices->indices.end (); ++idx)
223 for (idx = indices->indices.begin (); idx != indices->indices.end (); ++idx)
227 template <
typename Po
intT>
void
230 using namespace pcl::segmentation::grabcut;
231 const int number_of_indices =
static_cast<int> (indices_->size ());
234 graph_nodes_.clear ();
235 graph_nodes_.resize (indices_->size ());
236 int start = graph_.addNodes (indices_->size ());
237 for (
int idx = 0; idx < indices_->size (); ++idx)
239 graph_nodes_[idx] = start;
244 for (
int i_point = 0; i_point < number_of_indices; ++i_point)
246 int point_index = (*indices_) [i_point];
249 switch (trimap_[point_index])
253 fore =
static_cast<float> (-log (background_GMM_.probabilityDensity (image_->points[point_index])));
254 back =
static_cast<float> (-log (foreground_GMM_.probabilityDensity (image_->points[point_index])));
270 setTerminalWeights (graph_nodes_[i_point], fore, back);
274 for (
int i_point = 0; i_point < number_of_indices; ++i_point)
276 const NLinks &n_link = n_links_[i_point];
279 int point_index = (*indices_) [i_point];
280 std::vector<int>::const_iterator indices_it = n_link.
indices.begin ();
281 std::vector<float>::const_iterator weights_it = n_link.
weights.begin ();
282 for (; indices_it != n_link.
indices.end (); ++indices_it, ++weights_it)
284 if (*indices_it != point_index)
286 addEdge (graph_nodes_[i_point], graph_nodes_[*indices_it], *weights_it, *weights_it);
293 template <
typename Po
intT>
void
296 const int number_of_indices =
static_cast<int> (indices_->size ());
297 for (
int i_point = 0; i_point < number_of_indices; ++i_point)
299 NLinks &n_link = n_links_[i_point];
302 int point_index = (*indices_) [i_point];
303 std::vector<int>::const_iterator indices_it = n_link.
indices.begin ();
304 std::vector<float>::const_iterator dists_it = n_link.
dists.begin ();
305 std::vector<float>::iterator weights_it = n_link.
weights.begin ();
306 for (; indices_it != n_link.
indices.end (); ++indices_it, ++dists_it, ++weights_it)
308 if (*indices_it != point_index)
311 float color_distance = *weights_it;
313 *weights_it =
static_cast<float> (lambda_ * exp (-beta_ * color_distance) / sqrt (*dists_it));
320 template <
typename Po
intT>
void
324 std::size_t edges = 0;
326 const int number_of_indices =
static_cast<int> (indices_->size ());
328 for (
int i_point = 0; i_point < number_of_indices; i_point++)
330 int point_index = (*indices_)[i_point];
331 const PointT& point = input_->points [point_index];
334 NLinks &links = n_links_[i_point];
335 int found = tree_->nearestKSearch (point, nb_neighbours_, links.
indices, links.
dists);
340 for (std::vector<int>::const_iterator nn_it = links.
indices.begin (); nn_it != links.
indices.end (); ++nn_it)
342 if (*nn_it != point_index)
345 links.
weights.push_back (color_distance);
346 result+= color_distance;
355 std::cout <<
"result " << result << std::endl;
356 std::cout <<
"edges " << edges << std::endl;
357 beta_ = 1e5 / (2*result / edges);
358 std::cout <<
"beta " << beta_ << std::endl;
361 template <
typename Po
intT>
void
367 template <
typename Po
intT>
void
370 using namespace pcl::segmentation::grabcut;
373 clusters[0].indices.reserve (indices_->size ());
374 clusters[1].indices.reserve (indices_->size ());
376 assert (hard_segmentation_.size () == indices_->size ());
377 const int indices_size =
static_cast<int> (indices_->size ());
378 for (
int i = 0; i < indices_size; ++i)
380 clusters[1].indices.push_back (i);
382 clusters[0].indices.push_back (i);