Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * 00006 * All rights reserved. 00007 * 00008 * Redistribution and use in source and binary forms, with or without 00009 * modification, are permitted provided that the following conditions 00010 * are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of the copyright holder(s) nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 00034 * 00035 * $Id:$ 00036 * 00037 */ 00038 00039 #ifndef PCL_SEGMENTATION_MIN_CUT_SEGMENTATION_HPP_ 00040 #define PCL_SEGMENTATION_MIN_CUT_SEGMENTATION_HPP_ 00041 00042 #include <pcl/segmentation/boost.h> 00043 #include <pcl/segmentation/min_cut_segmentation.h> 00044 #include <pcl/search/search.h> 00045 #include <pcl/search/kdtree.h> 00046 #include <stdlib.h> 00047 #include <cmath> 00048 00049 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00050 template <typename PointT> 00051 pcl::MinCutSegmentation<PointT>::MinCutSegmentation () : 00052 inverse_sigma_ (16.0), 00053 binary_potentials_are_valid_ (false), 00054 epsilon_ (0.0001), 00055 radius_ (16.0), 00056 unary_potentials_are_valid_ (false), 00057 source_weight_ (0.8), 00058 search_ (), 00059 number_of_neighbours_ (14), 00060 graph_is_valid_ (false), 00061 foreground_points_ (0), 00062 background_points_ (0), 00063 clusters_ (0), 00064 graph_ (), 00065 capacity_ (), 00066 reverse_edges_ (), 00067 vertices_ (0), 00068 edge_marker_ (0), 00069 source_ (),///////////////////////////////// 00070 sink_ (),/////////////////////////////////// 00071 max_flow_ (0.0) 00072 { 00073 } 00074 00075 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00076 template <typename PointT> 00077 pcl::MinCutSegmentation<PointT>::~MinCutSegmentation () 00078 { 00079 if (search_ != 0) 00080 search_.reset (); 00081 if (graph_ != 0) 00082 graph_.reset (); 00083 if (capacity_ != 0) 00084 capacity_.reset (); 00085 if (reverse_edges_ != 0) 00086 reverse_edges_.reset (); 00087 00088 foreground_points_.clear (); 00089 background_points_.clear (); 00090 clusters_.clear (); 00091 vertices_.clear (); 00092 edge_marker_.clear (); 00093 } 00094 00095 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00096 template <typename PointT> void 00097 pcl::MinCutSegmentation<PointT>::setInputCloud (const PointCloudConstPtr &cloud) 00098 { 00099 input_ = cloud; 00100 graph_is_valid_ = false; 00101 unary_potentials_are_valid_ = false; 00102 binary_potentials_are_valid_ = false; 00103 } 00104 00105 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00106 template <typename PointT> double 00107 pcl::MinCutSegmentation<PointT>::getSigma () const 00108 { 00109 return (pow (1.0 / inverse_sigma_, 0.5)); 00110 } 00111 00112 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00113 template <typename PointT> void 00114 pcl::MinCutSegmentation<PointT>::setSigma (double sigma) 00115 { 00116 if (sigma > epsilon_) 00117 { 00118 inverse_sigma_ = 1.0 / (sigma * sigma); 00119 binary_potentials_are_valid_ = false; 00120 } 00121 } 00122 00123 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00124 template <typename PointT> double 00125 pcl::MinCutSegmentation<PointT>::getRadius () const 00126 { 00127 return (pow (radius_, 0.5)); 00128 } 00129 00130 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00131 template <typename PointT> void 00132 pcl::MinCutSegmentation<PointT>::setRadius (double radius) 00133 { 00134 if (radius > epsilon_) 00135 { 00136 radius_ = radius * radius; 00137 unary_potentials_are_valid_ = false; 00138 } 00139 } 00140 00141 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00142 template <typename PointT> double 00143 pcl::MinCutSegmentation<PointT>::getSourceWeight () const 00144 { 00145 return (source_weight_); 00146 } 00147 00148 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00149 template <typename PointT> void 00150 pcl::MinCutSegmentation<PointT>::setSourceWeight (double weight) 00151 { 00152 if (weight > epsilon_) 00153 { 00154 source_weight_ = weight; 00155 unary_potentials_are_valid_ = false; 00156 } 00157 } 00158 00159 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00160 template <typename PointT> typename pcl::MinCutSegmentation<PointT>::KdTreePtr 00161 pcl::MinCutSegmentation<PointT>::getSearchMethod () const 00162 { 00163 return (search_); 00164 } 00165 00166 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00167 template <typename PointT> void 00168 pcl::MinCutSegmentation<PointT>::setSearchMethod (const KdTreePtr& tree) 00169 { 00170 if (search_ != 0) 00171 search_.reset (); 00172 00173 search_ = tree; 00174 } 00175 00176 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00177 template <typename PointT> unsigned int 00178 pcl::MinCutSegmentation<PointT>::getNumberOfNeighbours () const 00179 { 00180 return (number_of_neighbours_); 00181 } 00182 00183 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00184 template <typename PointT> void 00185 pcl::MinCutSegmentation<PointT>::setNumberOfNeighbours (unsigned int neighbour_number) 00186 { 00187 if (number_of_neighbours_ != neighbour_number && neighbour_number != 0) 00188 { 00189 number_of_neighbours_ = neighbour_number; 00190 graph_is_valid_ = false; 00191 unary_potentials_are_valid_ = false; 00192 binary_potentials_are_valid_ = false; 00193 } 00194 } 00195 00196 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00197 template <typename PointT> std::vector<PointT, Eigen::aligned_allocator<PointT> > 00198 pcl::MinCutSegmentation<PointT>::getForegroundPoints () const 00199 { 00200 return (foreground_points_); 00201 } 00202 00203 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00204 template <typename PointT> void 00205 pcl::MinCutSegmentation<PointT>::setForegroundPoints (typename pcl::PointCloud<PointT>::Ptr foreground_points) 00206 { 00207 foreground_points_.clear (); 00208 foreground_points_.reserve (foreground_points->points.size ()); 00209 for (size_t i_point = 0; i_point < foreground_points->points.size (); i_point++) 00210 foreground_points_.push_back (foreground_points->points[i_point]); 00211 00212 unary_potentials_are_valid_ = false; 00213 } 00214 00215 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00216 template <typename PointT> std::vector<PointT, Eigen::aligned_allocator<PointT> > 00217 pcl::MinCutSegmentation<PointT>::getBackgroundPoints () const 00218 { 00219 return (background_points_); 00220 } 00221 00222 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00223 template <typename PointT> void 00224 pcl::MinCutSegmentation<PointT>::setBackgroundPoints (typename pcl::PointCloud<PointT>::Ptr background_points) 00225 { 00226 background_points_.clear (); 00227 background_points_.reserve (background_points->points.size ()); 00228 for (size_t i_point = 0; i_point < background_points->points.size (); i_point++) 00229 background_points_.push_back (background_points->points[i_point]); 00230 00231 unary_potentials_are_valid_ = false; 00232 } 00233 00234 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00235 template <typename PointT> void 00236 pcl::MinCutSegmentation<PointT>::extract (std::vector <pcl::PointIndices>& clusters) 00237 { 00238 clusters.clear (); 00239 00240 bool segmentation_is_possible = initCompute (); 00241 if ( !segmentation_is_possible ) 00242 { 00243 deinitCompute (); 00244 return; 00245 } 00246 00247 if ( graph_is_valid_ && unary_potentials_are_valid_ && binary_potentials_are_valid_ ) 00248 { 00249 clusters.reserve (clusters_.size ()); 00250 std::copy (clusters_.begin (), clusters_.end (), std::back_inserter (clusters)); 00251 deinitCompute (); 00252 return; 00253 } 00254 00255 clusters_.clear (); 00256 bool success = true; 00257 00258 if ( !graph_is_valid_ ) 00259 { 00260 success = buildGraph (); 00261 if (success == false) 00262 { 00263 deinitCompute (); 00264 return; 00265 } 00266 graph_is_valid_ = true; 00267 unary_potentials_are_valid_ = true; 00268 binary_potentials_are_valid_ = true; 00269 } 00270 00271 if ( !unary_potentials_are_valid_ ) 00272 { 00273 success = recalculateUnaryPotentials (); 00274 if (success == false) 00275 { 00276 deinitCompute (); 00277 return; 00278 } 00279 unary_potentials_are_valid_ = true; 00280 } 00281 00282 if ( !binary_potentials_are_valid_ ) 00283 { 00284 success = recalculateBinaryPotentials (); 00285 if (success == false) 00286 { 00287 deinitCompute (); 00288 return; 00289 } 00290 binary_potentials_are_valid_ = true; 00291 } 00292 00293 //IndexMap index_map = boost::get (boost::vertex_index, *graph_); 00294 ResidualCapacityMap residual_capacity = boost::get (boost::edge_residual_capacity, *graph_); 00295 00296 max_flow_ = boost::boykov_kolmogorov_max_flow (*graph_, source_, sink_); 00297 00298 assembleLabels (residual_capacity); 00299 00300 clusters.reserve (clusters_.size ()); 00301 std::copy (clusters_.begin (), clusters_.end (), std::back_inserter (clusters)); 00302 00303 deinitCompute (); 00304 } 00305 00306 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00307 template <typename PointT> double 00308 pcl::MinCutSegmentation<PointT>::getMaxFlow () const 00309 { 00310 return (max_flow_); 00311 } 00312 00313 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00314 template <typename PointT> typename boost::shared_ptr<typename pcl::MinCutSegmentation<PointT>::mGraph> 00315 pcl::MinCutSegmentation<PointT>::getGraph () const 00316 { 00317 return (graph_); 00318 } 00319 00320 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00321 template <typename PointT> bool 00322 pcl::MinCutSegmentation<PointT>::buildGraph () 00323 { 00324 int number_of_points = static_cast<int> (input_->points.size ()); 00325 int number_of_indices = static_cast<int> (indices_->size ()); 00326 00327 if (input_->points.size () == 0 || number_of_points == 0 || foreground_points_.empty () == true ) 00328 return (false); 00329 00330 if (search_ == 0) 00331 search_ = boost::shared_ptr<pcl::search::Search<PointT> > (new pcl::search::KdTree<PointT>); 00332 00333 graph_.reset (); 00334 graph_ = boost::shared_ptr< mGraph > (new mGraph ()); 00335 00336 capacity_.reset (); 00337 capacity_ = boost::shared_ptr<CapacityMap> (new CapacityMap ()); 00338 *capacity_ = boost::get (boost::edge_capacity, *graph_); 00339 00340 reverse_edges_.reset (); 00341 reverse_edges_ = boost::shared_ptr<ReverseEdgeMap> (new ReverseEdgeMap ()); 00342 *reverse_edges_ = boost::get (boost::edge_reverse, *graph_); 00343 00344 VertexDescriptor vertex_descriptor(0); 00345 vertices_.clear (); 00346 vertices_.resize (number_of_points + 2, vertex_descriptor); 00347 00348 std::set<int> out_edges_marker; 00349 edge_marker_.clear (); 00350 edge_marker_.resize (number_of_points + 2, out_edges_marker); 00351 00352 for (int i_point = 0; i_point < number_of_points + 2; i_point++) 00353 vertices_[i_point] = boost::add_vertex (*graph_); 00354 00355 source_ = vertices_[number_of_points]; 00356 sink_ = vertices_[number_of_points + 1]; 00357 00358 for (int i_point = 0; i_point < number_of_indices; i_point++) 00359 { 00360 int point_index = (*indices_)[i_point]; 00361 double source_weight = 0.0; 00362 double sink_weight = 0.0; 00363 calculateUnaryPotential (point_index, source_weight, sink_weight); 00364 addEdge (static_cast<int> (source_), point_index, source_weight); 00365 addEdge (point_index, static_cast<int> (sink_), sink_weight); 00366 } 00367 00368 std::vector<int> neighbours; 00369 std::vector<float> distances; 00370 search_->setInputCloud (input_, indices_); 00371 for (int i_point = 0; i_point < number_of_indices; i_point++) 00372 { 00373 int point_index = (*indices_)[i_point]; 00374 search_->nearestKSearch (i_point, number_of_neighbours_, neighbours, distances); 00375 for (size_t i_nghbr = 1; i_nghbr < neighbours.size (); i_nghbr++) 00376 { 00377 double weight = calculateBinaryPotential (point_index, neighbours[i_nghbr]); 00378 addEdge (point_index, neighbours[i_nghbr], weight); 00379 addEdge (neighbours[i_nghbr], point_index, weight); 00380 } 00381 neighbours.clear (); 00382 distances.clear (); 00383 } 00384 00385 return (true); 00386 } 00387 00388 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00389 template <typename PointT> void 00390 pcl::MinCutSegmentation<PointT>::calculateUnaryPotential (int point, double& source_weight, double& sink_weight) const 00391 { 00392 double min_dist_to_foreground = std::numeric_limits<double>::max (); 00393 //double min_dist_to_background = std::numeric_limits<double>::max (); 00394 double closest_foreground_point[2]; 00395 closest_foreground_point[0] = closest_foreground_point[1] = 0; 00396 //double closest_background_point[] = {0.0, 0.0}; 00397 double initial_point[] = {0.0, 0.0}; 00398 00399 initial_point[0] = input_->points[point].x; 00400 initial_point[1] = input_->points[point].y; 00401 00402 for (size_t i_point = 0; i_point < foreground_points_.size (); i_point++) 00403 { 00404 double dist = 0.0; 00405 dist += (foreground_points_[i_point].x - initial_point[0]) * (foreground_points_[i_point].x - initial_point[0]); 00406 dist += (foreground_points_[i_point].y - initial_point[1]) * (foreground_points_[i_point].y - initial_point[1]); 00407 if (min_dist_to_foreground > dist) 00408 { 00409 min_dist_to_foreground = dist; 00410 closest_foreground_point[0] = foreground_points_[i_point].x; 00411 closest_foreground_point[1] = foreground_points_[i_point].y; 00412 } 00413 } 00414 00415 sink_weight = pow (min_dist_to_foreground / radius_, 0.5); 00416 00417 source_weight = source_weight_; 00418 return; 00419 /* 00420 if (background_points_.size () == 0) 00421 return; 00422 00423 for (int i_point = 0; i_point < background_points_.size (); i_point++) 00424 { 00425 double dist = 0.0; 00426 dist += (background_points_[i_point].x - initial_point[0]) * (background_points_[i_point].x - initial_point[0]); 00427 dist += (background_points_[i_point].y - initial_point[1]) * (background_points_[i_point].y - initial_point[1]); 00428 if (min_dist_to_background > dist) 00429 { 00430 min_dist_to_background = dist; 00431 closest_background_point[0] = background_points_[i_point].x; 00432 closest_background_point[1] = background_points_[i_point].y; 00433 } 00434 } 00435 00436 if (min_dist_to_background <= epsilon_) 00437 { 00438 source_weight = 0.0; 00439 sink_weight = 1.0; 00440 return; 00441 } 00442 00443 source_weight = 1.0 / (1.0 + pow (min_dist_to_background / min_dist_to_foreground, 0.5)); 00444 sink_weight = 1 - source_weight; 00445 */ 00446 } 00447 00448 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00449 template <typename PointT> bool 00450 pcl::MinCutSegmentation<PointT>::addEdge (int source, int target, double weight) 00451 { 00452 std::set<int>::iterator iter_out = edge_marker_[source].find (target); 00453 if ( iter_out != edge_marker_[source].end () ) 00454 return (false); 00455 00456 EdgeDescriptor edge; 00457 EdgeDescriptor reverse_edge; 00458 bool edge_was_added, reverse_edge_was_added; 00459 00460 boost::tie (edge, edge_was_added) = boost::add_edge ( vertices_[source], vertices_[target], *graph_ ); 00461 boost::tie (reverse_edge, reverse_edge_was_added) = boost::add_edge ( vertices_[target], vertices_[source], *graph_ ); 00462 if ( !edge_was_added || !reverse_edge_was_added ) 00463 return (false); 00464 00465 (*capacity_)[edge] = weight; 00466 (*capacity_)[reverse_edge] = 0.0; 00467 (*reverse_edges_)[edge] = reverse_edge; 00468 (*reverse_edges_)[reverse_edge] = edge; 00469 edge_marker_[source].insert (target); 00470 00471 return (true); 00472 } 00473 00474 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00475 template <typename PointT> double 00476 pcl::MinCutSegmentation<PointT>::calculateBinaryPotential (int source, int target) const 00477 { 00478 double weight = 0.0; 00479 double distance = 0.0; 00480 distance += (input_->points[source].x - input_->points[target].x) * (input_->points[source].x - input_->points[target].x); 00481 distance += (input_->points[source].y - input_->points[target].y) * (input_->points[source].y - input_->points[target].y); 00482 distance += (input_->points[source].z - input_->points[target].z) * (input_->points[source].z - input_->points[target].z); 00483 distance *= inverse_sigma_; 00484 weight = exp (-distance); 00485 00486 return (weight); 00487 } 00488 00489 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00490 template <typename PointT> bool 00491 pcl::MinCutSegmentation<PointT>::recalculateUnaryPotentials () 00492 { 00493 OutEdgeIterator src_edge_iter; 00494 OutEdgeIterator src_edge_end; 00495 std::pair<EdgeDescriptor, bool> sink_edge; 00496 00497 for (boost::tie (src_edge_iter, src_edge_end) = boost::out_edges (source_, *graph_); src_edge_iter != src_edge_end; src_edge_iter++) 00498 { 00499 double source_weight = 0.0; 00500 double sink_weight = 0.0; 00501 sink_edge.second = false; 00502 calculateUnaryPotential (static_cast<int> (boost::target (*src_edge_iter, *graph_)), source_weight, sink_weight); 00503 sink_edge = boost::lookup_edge (boost::target (*src_edge_iter, *graph_), sink_, *graph_); 00504 if (!sink_edge.second) 00505 return (false); 00506 00507 (*capacity_)[*src_edge_iter] = source_weight; 00508 (*capacity_)[sink_edge.first] = sink_weight; 00509 } 00510 00511 return (true); 00512 } 00513 00514 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00515 template <typename PointT> bool 00516 pcl::MinCutSegmentation<PointT>::recalculateBinaryPotentials () 00517 { 00518 int number_of_points = static_cast<int> (indices_->size ()); 00519 00520 VertexIterator vertex_iter; 00521 VertexIterator vertex_end; 00522 OutEdgeIterator edge_iter; 00523 OutEdgeIterator edge_end; 00524 00525 std::vector< std::set<VertexDescriptor> > edge_marker; 00526 std::set<VertexDescriptor> out_edges_marker; 00527 edge_marker.clear (); 00528 edge_marker.resize (number_of_points + 2, out_edges_marker); 00529 00530 for (boost::tie (vertex_iter, vertex_end) = boost::vertices (*graph_); vertex_iter != vertex_end; vertex_iter++) 00531 { 00532 VertexDescriptor source_vertex = *vertex_iter; 00533 if (source_vertex == source_ || source_vertex == sink_) 00534 continue; 00535 for (boost::tie (edge_iter, edge_end) = boost::out_edges (source_vertex, *graph_); edge_iter != edge_end; edge_iter++) 00536 { 00537 //If this is not the edge of the graph, but the reverse fictitious edge that is needed for the algorithm then continue 00538 EdgeDescriptor reverse_edge = (*reverse_edges_)[*edge_iter]; 00539 if ((*capacity_)[reverse_edge] != 0.0) 00540 continue; 00541 00542 //If we already changed weight for this edge then continue 00543 VertexDescriptor target_vertex = boost::target (*edge_iter, *graph_); 00544 std::set<VertexDescriptor>::iterator iter_out = edge_marker[static_cast<int> (source_vertex)].find (target_vertex); 00545 if ( iter_out != edge_marker[static_cast<int> (source_vertex)].end () ) 00546 continue; 00547 00548 if (target_vertex != source_ && target_vertex != sink_) 00549 { 00550 //Change weight and remember that this edges were updated 00551 double weight = calculateBinaryPotential (static_cast<int> (target_vertex), static_cast<int> (source_vertex)); 00552 (*capacity_)[*edge_iter] = weight; 00553 edge_marker[static_cast<int> (source_vertex)].insert (target_vertex); 00554 } 00555 } 00556 } 00557 00558 return (true); 00559 } 00560 00561 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00562 template <typename PointT> void 00563 pcl::MinCutSegmentation<PointT>::assembleLabels (ResidualCapacityMap& residual_capacity) 00564 { 00565 std::vector<int> labels; 00566 labels.resize (input_->points.size (), 0); 00567 int number_of_indices = static_cast<int> (indices_->size ()); 00568 for (int i_point = 0; i_point < number_of_indices; i_point++) 00569 labels[(*indices_)[i_point]] = 1; 00570 00571 clusters_.clear (); 00572 00573 pcl::PointIndices segment; 00574 clusters_.resize (2, segment); 00575 00576 OutEdgeIterator edge_iter, edge_end; 00577 for ( boost::tie (edge_iter, edge_end) = boost::out_edges (source_, *graph_); edge_iter != edge_end; edge_iter++ ) 00578 { 00579 if (labels[edge_iter->m_target] == 1) 00580 { 00581 if (residual_capacity[*edge_iter] > epsilon_) 00582 clusters_[1].indices.push_back (static_cast<int> (edge_iter->m_target)); 00583 else 00584 clusters_[0].indices.push_back (static_cast<int> (edge_iter->m_target)); 00585 } 00586 } 00587 } 00588 00589 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00590 template <typename PointT> pcl::PointCloud<pcl::PointXYZRGB>::Ptr 00591 pcl::MinCutSegmentation<PointT>::getColoredCloud () 00592 { 00593 pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud; 00594 00595 if (!clusters_.empty ()) 00596 { 00597 int num_of_pts_in_first_cluster = static_cast<int> (clusters_[0].indices.size ()); 00598 int num_of_pts_in_second_cluster = static_cast<int> (clusters_[1].indices.size ()); 00599 int number_of_points = num_of_pts_in_first_cluster + num_of_pts_in_second_cluster; 00600 colored_cloud = (new pcl::PointCloud<pcl::PointXYZRGB>)->makeShared (); 00601 unsigned char foreground_color[3] = {255, 255, 255}; 00602 unsigned char background_color[3] = {255, 0, 0}; 00603 colored_cloud->width = number_of_points; 00604 colored_cloud->height = 1; 00605 colored_cloud->is_dense = input_->is_dense; 00606 00607 pcl::PointXYZRGB point; 00608 int point_index = 0; 00609 for (int i_point = 0; i_point < num_of_pts_in_first_cluster; i_point++) 00610 { 00611 point_index = clusters_[0].indices[i_point]; 00612 point.x = *(input_->points[point_index].data); 00613 point.y = *(input_->points[point_index].data + 1); 00614 point.z = *(input_->points[point_index].data + 2); 00615 point.r = background_color[0]; 00616 point.g = background_color[1]; 00617 point.b = background_color[2]; 00618 colored_cloud->points.push_back (point); 00619 } 00620 00621 for (int i_point = 0; i_point < num_of_pts_in_second_cluster; i_point++) 00622 { 00623 point_index = clusters_[1].indices[i_point]; 00624 point.x = *(input_->points[point_index].data); 00625 point.y = *(input_->points[point_index].data + 1); 00626 point.z = *(input_->points[point_index].data + 2); 00627 point.r = foreground_color[0]; 00628 point.g = foreground_color[1]; 00629 point.b = foreground_color[2]; 00630 colored_cloud->points.push_back (point); 00631 } 00632 } 00633 00634 return (colored_cloud); 00635 } 00636 00637 #define PCL_INSTANTIATE_MinCutSegmentation(T) template class pcl::MinCutSegmentation<T>; 00638 00639 #endif // PCL_SEGMENTATION_MIN_CUT_SEGMENTATION_HPP_