Point Cloud Library (PCL)
1.7.0
|
00001 /////////////////////////////////////////////////////// 00002 // grabCut->initialize (xstart, ystart, xend, yend); // 00003 // grabCut->fitGMMs (); // 00004 /////////////////////////////////////////////////////// 00005 #ifndef PCL_SEGMENTATION_IMPL_GRABCUT_HPP 00006 #define PCL_SEGMENTATION_IMPL_GRABCUT_HPP 00007 00008 #include <pcl/search/organized.h> 00009 #include <pcl/search/kdtree.h> 00010 #include <pcl/common/distances.h> 00011 00012 namespace pcl 00013 { 00014 template <> 00015 float squaredEuclideanDistance (const pcl::segmentation::grabcut::Color &c1, 00016 const pcl::segmentation::grabcut::Color &c2) 00017 { 00018 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)); 00019 } 00020 } 00021 00022 template <typename PointT> void 00023 pcl::GrabCut<PointT>::setInputCloud (const PointCloudConstPtr &cloud) 00024 { 00025 input_ = cloud; 00026 } 00027 00028 template <typename PointT> bool 00029 pcl::GrabCut<PointT>::initCompute () 00030 { 00031 using namespace pcl::segmentation::grabcut; 00032 if (!pcl::PCLBase<PointT>::initCompute ()) 00033 { 00034 PCL_ERROR ("[pcl::GrabCut::initCompute ()] Init failed!"); 00035 return (false); 00036 } 00037 00038 if (!input_->isOrganized ()) 00039 { 00040 PCL_ERROR ("[pcl::GrabCut::initCompute ()] Need an organized point cloud to proceed!"); 00041 return (false); 00042 } 00043 00044 std::vector<pcl::PCLPointField> in_fields_; 00045 if ((pcl::getFieldIndex<PointT> (*input_, "rgb", in_fields_) == -1) && 00046 (pcl::getFieldIndex<PointT> (*input_, "rgba", in_fields_) == -1)) 00047 { 00048 PCL_ERROR ("[pcl::GrabCut::initCompute ()] No RGB data available, aborting!"); 00049 return (false); 00050 } 00051 00052 // Initialize the working image 00053 image_.reset (new Image (input_->width, input_->height)); 00054 for (std::size_t i = 0; i < input_->size (); ++i) 00055 { 00056 (*image_) [i] = Color (input_->points[i]); 00057 } 00058 width_ = image_->width; 00059 height_ = image_->height; 00060 00061 // Initialize the spatial locator 00062 if (!tree_) 00063 { 00064 if (input_->isOrganized ()) 00065 tree_.reset (new pcl::search::OrganizedNeighbor<PointT> ()); 00066 else 00067 tree_.reset (new pcl::search::KdTree<PointT> (false)); 00068 tree_->setInputCloud (input_); 00069 } 00070 const std::size_t indices_size = indices_->size (); 00071 trimap_ = std::vector<segmentation::grabcut::TrimapValue> (indices_size, TrimapUnknown); 00072 hard_segmentation_ = std::vector<segmentation::grabcut::SegmentationValue> (indices_size, 00073 SegmentationBackground); 00074 GMM_component_.resize (indices_size); 00075 n_links_.resize (indices_size); 00076 // soft_segmentation_ = 0; // Not yet implemented 00077 foreground_GMM_.resize (K_); 00078 background_GMM_.resize (K_); 00079 //set some constants 00080 computeL (); 00081 computeBeta (); 00082 computeNLinks (); 00083 00084 initialized_ = false; 00085 return (true); 00086 } 00087 00088 template <typename PointT> void 00089 pcl::GrabCut<PointT>::addEdge (vertex_descriptor v1, vertex_descriptor v2, float capacity, float rev_capacity) 00090 { 00091 graph_.addEdge (v1, v2, capacity, rev_capacity); 00092 } 00093 00094 template <typename PointT> void 00095 pcl::GrabCut<PointT>::setTerminalWeights (vertex_descriptor v, float source_capacity, float sink_capacity) 00096 { 00097 graph_.addSourceEdge (v, source_capacity); 00098 graph_.addTargetEdge (v, sink_capacity); 00099 } 00100 00101 // template <typename PointT> void 00102 // pcl::GrabCut<PointT>::setBackgroundPointsIndices (int x1, int y1, int x2, int y2) 00103 // { 00104 // using namespace pcl::segmentation::grabcut; 00105 00106 // // Step 1: User creates inital Trimap with rectangle, Background outside, Unknown inside 00107 // fill (trimap_.begin (), trimap_.end (), TrimapBackground); 00108 // fillRectangle (trimap_, width_, height_, x1, y1, x2, y2, TrimapUnknown); 00109 00110 // // Step 2: Initial segmentation, Background where Trimap is Background, Foreground where Trimap is Unknown. 00111 // fill (hard_segmentation_.begin (), hard_segmentation_.end (), SegmentationBackground); 00112 // fillRectangle (hard_segmentation_, width_, height_, x1, y1, x2, y2, SegmentationForeground); 00113 // if (!initialized_) 00114 // { 00115 // fitGMMs (); 00116 // initialized_ = true; 00117 // } 00118 // } 00119 00120 template <typename PointT> void 00121 pcl::GrabCut<PointT>::setBackgroundPointsIndices (const PointIndicesConstPtr &indices) 00122 { 00123 using namespace pcl::segmentation::grabcut; 00124 if (!initCompute ()) 00125 return; 00126 00127 std::fill (trimap_.begin (), trimap_.end (), TrimapBackground); 00128 std::fill (hard_segmentation_.begin (), hard_segmentation_.end (), SegmentationBackground); 00129 for (std::vector<int>::const_iterator idx = indices->indices.begin (); idx != indices->indices.end (); ++idx) 00130 { 00131 trimap_[*idx] = TrimapUnknown; 00132 hard_segmentation_[*idx] = SegmentationForeground; 00133 } 00134 00135 if (!initialized_) 00136 { 00137 fitGMMs (); 00138 initialized_ = true; 00139 } 00140 } 00141 00142 template <typename PointT> void 00143 pcl::GrabCut<PointT>::fitGMMs () 00144 { 00145 // Step 3: Build GMMs using Orchard-Bouman clustering algorithm 00146 buildGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_); 00147 00148 // Initialize the graph for graphcut (do this here so that the T-Link debugging image will be initialized) 00149 initGraph (); 00150 } 00151 00152 template <typename PointT> int 00153 pcl::GrabCut<PointT>::refineOnce () 00154 { 00155 // Steps 4 and 5: Learn new GMMs from current segmentation 00156 learnGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_); 00157 00158 // Step 6: Run GraphCut and update segmentation 00159 initGraph (); 00160 00161 float flow = graph_.solve (); 00162 00163 int changed = updateHardSegmentation (); 00164 PCL_INFO ("%d pixels changed segmentation (max flow = %f)\n", changed, flow); 00165 00166 return (changed); 00167 } 00168 00169 template <typename PointT> void 00170 pcl::GrabCut<PointT>::refine () 00171 { 00172 std::size_t changed = indices_->size (); 00173 00174 while (changed) 00175 changed = refineOnce (); 00176 } 00177 00178 template <typename PointT> int 00179 pcl::GrabCut<PointT>::updateHardSegmentation () 00180 { 00181 using namespace pcl::segmentation::grabcut; 00182 00183 int changed = 0; 00184 00185 const int number_of_indices = static_cast<int> (indices_->size ()); 00186 for (int i_point = 0; i_point < number_of_indices; ++i_point) 00187 { 00188 SegmentationValue old_value = hard_segmentation_ [i_point]; 00189 00190 if (trimap_ [i_point] == TrimapBackground) 00191 hard_segmentation_ [i_point] = SegmentationBackground; 00192 else 00193 if (trimap_ [i_point] == TrimapForeground) 00194 hard_segmentation_ [i_point] = SegmentationForeground; 00195 else // TrimapUnknown 00196 { 00197 if (isSource (graph_nodes_[i_point])) 00198 hard_segmentation_ [i_point] = SegmentationForeground; 00199 else 00200 hard_segmentation_ [i_point] = SegmentationBackground; 00201 } 00202 00203 if (old_value != hard_segmentation_ [i_point]) 00204 ++changed; 00205 } 00206 return (changed); 00207 } 00208 00209 template <typename PointT> void 00210 pcl::GrabCut<PointT>::setTrimap (const PointIndicesConstPtr &indices, segmentation::grabcut::TrimapValue t) 00211 { 00212 using namespace pcl::segmentation::grabcut; 00213 std::vector<int>::const_iterator idx = indices->indices.begin (); 00214 for (; idx != indices->indices.end (); ++idx) 00215 trimap_[*idx] = t; 00216 00217 // Immediately set the hard segmentation as well so that the display will update. 00218 if (t == TrimapForeground) 00219 for (idx = indices->indices.begin (); idx != indices->indices.end (); ++idx) 00220 hard_segmentation_[*idx] = SegmentationForeground; 00221 else 00222 if (t == TrimapBackground) 00223 for (idx = indices->indices.begin (); idx != indices->indices.end (); ++idx) 00224 hard_segmentation_[*idx] = SegmentationBackground; 00225 } 00226 00227 template <typename PointT> void 00228 pcl::GrabCut<PointT>::initGraph () 00229 { 00230 using namespace pcl::segmentation::grabcut; 00231 const int number_of_indices = static_cast<int> (indices_->size ()); 00232 // Set up the graph (it can only be used once, so we have to recreate it each time the graph is updated) 00233 graph_.clear (); 00234 graph_nodes_.clear (); 00235 graph_nodes_.resize (indices_->size ()); 00236 int start = graph_.addNodes (indices_->size ()); 00237 for (int idx = 0; idx < indices_->size (); ++idx) 00238 { 00239 graph_nodes_[idx] = start; 00240 ++start; 00241 } 00242 00243 // Set T-Link weights 00244 for (int i_point = 0; i_point < number_of_indices; ++i_point) 00245 { 00246 int point_index = (*indices_) [i_point]; 00247 float back, fore; 00248 00249 switch (trimap_[point_index]) 00250 { 00251 case TrimapUnknown : 00252 { 00253 fore = static_cast<float> (-log (background_GMM_.probabilityDensity (image_->points[point_index]))); 00254 back = static_cast<float> (-log (foreground_GMM_.probabilityDensity (image_->points[point_index]))); 00255 break; 00256 } 00257 case TrimapBackground : 00258 { 00259 fore = 0; 00260 back = L_; 00261 break; 00262 } 00263 default : 00264 { 00265 fore = L_; 00266 back = 0; 00267 } 00268 } 00269 00270 setTerminalWeights (graph_nodes_[i_point], fore, back); 00271 } 00272 00273 // Set N-Link weights from precomputed values 00274 for (int i_point = 0; i_point < number_of_indices; ++i_point) 00275 { 00276 const NLinks &n_link = n_links_[i_point]; 00277 if (n_link.nb_links > 0) 00278 { 00279 int point_index = (*indices_) [i_point]; 00280 std::vector<int>::const_iterator indices_it = n_link.indices.begin (); 00281 std::vector<float>::const_iterator weights_it = n_link.weights.begin (); 00282 for (; indices_it != n_link.indices.end (); ++indices_it, ++weights_it) 00283 { 00284 if (*indices_it != point_index) 00285 { 00286 addEdge (graph_nodes_[i_point], graph_nodes_[*indices_it], *weights_it, *weights_it); 00287 } 00288 } 00289 } 00290 } 00291 } 00292 00293 template <typename PointT> void 00294 pcl::GrabCut<PointT>::computeNLinks () 00295 { 00296 const int number_of_indices = static_cast<int> (indices_->size ()); 00297 for (int i_point = 0; i_point < number_of_indices; ++i_point) 00298 { 00299 NLinks &n_link = n_links_[i_point]; 00300 if (n_link.nb_links > 0) 00301 { 00302 int point_index = (*indices_) [i_point]; 00303 std::vector<int>::const_iterator indices_it = n_link.indices.begin (); 00304 std::vector<float>::const_iterator dists_it = n_link.dists.begin (); 00305 std::vector<float>::iterator weights_it = n_link.weights.begin (); 00306 for (; indices_it != n_link.indices.end (); ++indices_it, ++dists_it, ++weights_it) 00307 { 00308 if (*indices_it != point_index) 00309 { 00310 // We saved the color distance previously at the computeBeta stage for optimization purpose 00311 float color_distance = *weights_it; 00312 // Set the real weight 00313 *weights_it = static_cast<float> (lambda_ * exp (-beta_ * color_distance) / sqrt (*dists_it)); 00314 } 00315 } 00316 } 00317 } 00318 } 00319 00320 template <typename PointT> void 00321 pcl::GrabCut<PointT>::computeBeta () 00322 { 00323 float result = 0; 00324 std::size_t edges = 0; 00325 00326 const int number_of_indices = static_cast<int> (indices_->size ()); 00327 00328 for (int i_point = 0; i_point < number_of_indices; i_point++) 00329 { 00330 int point_index = (*indices_)[i_point]; 00331 const PointT& point = input_->points [point_index]; 00332 if (pcl::isFinite (point)) 00333 { 00334 NLinks &links = n_links_[i_point]; 00335 int found = tree_->nearestKSearch (point, nb_neighbours_, links.indices, links.dists); 00336 if (found > 1) 00337 { 00338 links.nb_links = found - 1; 00339 links.weights.reserve (links.nb_links); 00340 for (std::vector<int>::const_iterator nn_it = links.indices.begin (); nn_it != links.indices.end (); ++nn_it) 00341 { 00342 if (*nn_it != point_index) 00343 { 00344 float color_distance = squaredEuclideanDistance (image_->points[point_index], image_->points[*nn_it]); 00345 links.weights.push_back (color_distance); 00346 result+= color_distance; 00347 ++edges; 00348 } 00349 else 00350 links.weights.push_back (0.f); 00351 } 00352 } 00353 } 00354 } 00355 std::cout << "result " << result << std::endl; 00356 std::cout << "edges " << edges << std::endl; 00357 beta_ = 1e5 / (2*result / edges); 00358 std::cout << "beta " << beta_ << std::endl; 00359 } 00360 00361 template <typename PointT> void 00362 pcl::GrabCut<PointT>::computeL () 00363 { 00364 L_ = 8*lambda_ + 1; 00365 } 00366 00367 template <typename PointT> void 00368 pcl::GrabCut<PointT>::extract (std::vector<pcl::PointIndices>& clusters) 00369 { 00370 using namespace pcl::segmentation::grabcut; 00371 clusters.clear (); 00372 clusters.resize (2); 00373 clusters[0].indices.reserve (indices_->size ()); 00374 clusters[1].indices.reserve (indices_->size ()); 00375 refine (); 00376 assert (hard_segmentation_.size () == indices_->size ()); 00377 const int indices_size = static_cast<int> (indices_->size ()); 00378 for (int i = 0; i < indices_size; ++i) 00379 if (hard_segmentation_[i] == SegmentationForeground) 00380 clusters[1].indices.push_back (i); 00381 else 00382 clusters[0].indices.push_back (i); 00383 } 00384 00385 #endif