Skip to content

Commit

Permalink
git-svn-id: svn+ssh://svn.pointclouds.org/pcl/trunk@4759 a9d63959-f2a…
Browse files Browse the repository at this point in the history 
…d-4865-b262-bf0e56cfafb6
  • Loading branch information
Sergey Ushakov committed Feb 26, 2012
1 parent 577280a commit 2155a08
Showing 1 changed file with 79 additions and 91 deletions.
170 changes: 79 additions & 91 deletions segmentation/include/pcl/segmentation/impl/region_growing.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,6 @@
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2009-2012, Willow Garage, Inc.
*
* All rights reserved.
*
Expand Down Expand Up @@ -33,6 +32,9 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author : Sergey Ushakov
* Email : mine_all_mine@bk.ru
*
*/

#ifndef _REGIONGROWING_HPP_
Expand All @@ -41,13 +43,10 @@
#include "pcl/segmentation/region_growing.h"

#include "pcl/search/search.h"
// We do not compute normals inside of segmentation. Please rewrite this piece of code:
//#include "pcl/features/normal_3d.h"
#include "pcl/search/search.h"
#include "pcl/search/kdtree.h"
#include "pcl/features/normal_3d.h"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#include <queue>

#include <list>
#include <math.h>
#include <time.h>
Expand Down Expand Up @@ -176,7 +175,7 @@ namespace pcl
return (cloud_for_segmentation_);
}

template <typename PointT> std::vector<std::list<int> >
template <typename PointT> std::vector<std::list<int>>
RegionGrowing<PointT>::getSegments () const
{
return (segments_);
Expand All @@ -191,9 +190,9 @@ namespace pcl
return (result);
}

//first of all we need to find out if this point belongs to cloud
// first of all we need to find out if this point belongs to cloud
bool point_was_found = false;
if (index < (int)cloud_for_segmentation_->points.size() && index >= 0)
if (index < cloud_for_segmentation_->points.size() && index >= 0)
{
point_was_found = true;
}
Expand All @@ -202,9 +201,9 @@ namespace pcl
{
if ( !segments_.empty() )
{
//if we have already made the segmentation, then find the segment
//to which this point belongs
std::vector<std::list<int> >::iterator i_segment;
// if we have already made the segmentation, then find the segment
// to which this point belongs
std::vector<std::list<int>>::iterator i_segment;
for (i_segment = segments_.begin(); i_segment != segments_.end(); i_segment++)
{
bool segment_was_found = false;
Expand All @@ -223,52 +222,38 @@ namespace pcl
{
break;
}
}//next segment
}//end if segments are not empty
}// next segment
}// end if segments are not empty
else
{
//if user didn't set search method
if (search_ == 0)
{
search_ = boost::shared_ptr< pcl::search::Search<PointT> >(new pcl::search::KdTree<PointT>);
}

search_->setInputCloud(cloud_for_segmentation_);

//if user didn't set input normals then they need to be computed
/** We do not compute normals inside of segmentation. Please rewrite this piece of code:
* if (normals_ == 0)
bool segmentation_is_possible = prepareForSegmentation();
if ( !segmentation_is_possible )
{
normals_ = (new pcl::PointCloud<pcl::Normal>)->makeShared();
pcl::NormalEstimation<PointT, pcl::Normal> normal_estimator;
normal_estimator.setInputCloud(cloud_for_segmentation_);
normal_estimator.setKSearch(30);
normal_estimator.compute(*normals_);
return (result);
}
*/
//if we haven't done the segmentation yet, then we need to grow the segment
// if we haven't done the segmentation yet, then we need to grow the segment
std::vector<int> point_is_used;
point_is_used.resize(cloud_for_segmentation_->points.size(), 0);
growRegion(index, point_is_used, result);
}
}//end if point was found
}// end if point was found

return (result);
}

template <typename PointT> std::list<int>
RegionGrowing<PointT>::getSegmentFromPoint (PointT point)
RegionGrowing<PointT>::getSegmentFromPoint (typename PointT point)
{
std::list<int> result;
if (cloud_for_segmentation_ == 0)
{
return (result);
}

//first of all we need to find out if this point belongs to cloud
// first of all we need to find out if this point belongs to cloud
bool point_was_found = false;
int index = 0;
for (size_t i = 0; i < cloud_for_segmentation_->points.size(); i++)
for (int i = 0; i < cloud_for_segmentation_->points.size(); i++)
{
if (cloud_for_segmentation_->points[i].x != point.x) continue;
if (cloud_for_segmentation_->points[i].x != point.x) continue;
Expand All @@ -283,9 +268,9 @@ namespace pcl
{
if ( !segments_.empty() )
{
//if we have already made the segmentation, then find the segment
//to which this point belongs
std::vector<std::list<int> >::iterator i_segment;
// if we have already made the segmentation, then find the segment
// to which this point belongs
std::vector<std::list<int>>::iterator i_segment;
for (i_segment = segments_.begin(); i_segment != segments_.end(); i_segment++)
{
bool segment_was_found = false;
Expand All @@ -304,35 +289,21 @@ namespace pcl
{
break;
}
}//next segment
}//end if segments are not empty
}// next segment
}// end if segments are not empty
else
{
//if user didn't set search method
if (search_ == 0)
bool segmentation_is_possible = prepareForSegmentation();
if ( !segmentation_is_possible )
{
search_ = boost::shared_ptr< pcl::search::Search<PointT> >(new pcl::search::KdTree<PointT>);
return (result);
}

search_->setInputCloud(cloud_for_segmentation_);

//if user didn't set input normals then they need to be computed
/** We do not compute normals inside of segmentation. Please rewrite this piece of code:
if (normals_ == 0)
{
normals_ = (new pcl::PointCloud<pcl::Normal>)->makeShared();
pcl::NormalEstimation<PointT, pcl::Normal> normal_estimator;
normal_estimator.setInputCloud(cloud_for_segmentation_);
normal_estimator.setKSearch(30);
normal_estimator.compute(*normals_);
}
*/
//if we haven't done the segmentation yet, then we need to grow the segment
// if we haven't done the segmentation yet, then we need to grow the segment
std::vector<int> point_is_used;
point_is_used.resize(cloud_for_segmentation_->points.size(), 0);
growRegion(index, point_is_used, result);
}
}//end if point was found
}// end if point was found

return (result);
}
Expand All @@ -348,7 +319,7 @@ namespace pcl

srand(static_cast<unsigned int>(time(0)));
std::vector<unsigned char> colors;
for (size_t i_segment = 0; i_segment < segments_.size(); i_segment++)
for (int i_segment = 0; i_segment < segments_.size(); i_segment++)
{
colors.push_back(rand() % 256);
colors.push_back(rand() % 256);
Expand All @@ -358,7 +329,7 @@ namespace pcl
colored_cloud->width = cloud_for_segmentation_->width;
colored_cloud->height = cloud_for_segmentation_->height;
colored_cloud->is_dense = cloud_for_segmentation_->is_dense;
for (size_t i_point = 0; i_point < cloud_for_segmentation_->points.size(); i_point++)
for (int i_point = 0; i_point < cloud_for_segmentation_->points.size(); i_point++)
{
pcl::PointXYZRGB point;
point.x = *(cloud_for_segmentation_->points[i_point].data);
Expand All @@ -367,7 +338,7 @@ namespace pcl
colored_cloud->points.push_back(point);
}

std::vector<std::list<int> >::iterator i_segment;
std::vector<std::list<int>>::iterator i_segment;
int next_color = 0;
for (i_segment = segments_.begin(); i_segment != segments_.end(); i_segment++)
{
Expand Down Expand Up @@ -447,41 +418,58 @@ namespace pcl
template <typename PointT> unsigned int
RegionGrowing<PointT>::segmentPoints ()
{
if ( cloud_for_segmentation_ == 0 )
segments_.clear();
unsigned int number_of_segments = 0;
number_of_segments = applySmoothRegionGrowingAlgorithm();
return (number_of_segments);
}

template <typename PointT> bool
RegionGrowing<PointT>::prepareForSegmentation ()
{
// if user forgot to pass points or normals
if ( cloud_for_segmentation_ == 0 || normals_ == 0 )
{
return (0);
return (false);
}
else

// if number of points an number of normals are different
if (cloud_for_segmentation_->points.size() != normals_->points.size())
{
unsigned int number_of_segments = 0;
number_of_segments = applySmoothRegionGrowingAlgorithm();
return (number_of_segments);
return (false);
}
}

template <typename PointT> unsigned int
RegionGrowing<PointT>::applySmoothRegionGrowingAlgorithm ()
{
//if user didn't set search method
// if the cloud is empty
if ( cloud_for_segmentation_->points.size() == 0 || normals_->points.size() == 0 )
{
return (false);
}

// if user passed wrong parameters
if ( neighbour_number_ == 0 || residual_threshold_ <= 0.0f )
{
return (false);
}

// if user didn't set search method
if (search_ == 0)
{
search_ = boost::shared_ptr< pcl::search::Search<PointT> >(new pcl::search::KdTree<PointT>);
}

search_->setInputCloud(cloud_for_segmentation_);

//if user didn't set input normals then they need to be computed
/** We do not compute normals inside of segmentation. Please rewrite this piece of code:
if (normals_ == 0)
return (true);
}

template <typename PointT> unsigned int
RegionGrowing<PointT>::applySmoothRegionGrowingAlgorithm ()
{
bool segmentation_is_possible = prepareForSegmentation();
if ( !segmentation_is_possible )
{
normals_ = (new pcl::PointCloud<pcl::Normal>)->makeShared();
pcl::NormalEstimation<PointT, pcl::Normal> normal_estimator;
normal_estimator.setInputCloud(cloud_for_segmentation_);
normal_estimator.setKSearch(30);
normal_estimator.compute(*normals_);
return (0);
}
*/
segments_.clear();

int num_of_pts = static_cast<int>( cloud_for_segmentation_->points.size() );

Expand All @@ -491,7 +479,7 @@ namespace pcl
std::vector<int> point_is_used;
point_is_used.resize(num_of_pts, 0);

for (int i_point = 0; i_point < num_of_pts; i_point++)
for (size_t i_point = 0; i_point < num_of_pts; i_point++)
{
point_residual[i_point] = normals_->points[i_point].curvature;
}
Expand Down Expand Up @@ -539,7 +527,7 @@ namespace pcl
std::vector<float> nghbr_distances;
search_->nearestKSearch(curr_seed, neighbour_number_, nghbr_indices, nghbr_distances);

for (size_t i_nghbr = 0; i_nghbr < nghbr_indices.size(); i_nghbr++)
for (int i_nghbr = 0; i_nghbr < nghbr_indices.size(); i_nghbr++)
{
int index = nghbr_indices[i_nghbr];
if (point_is_used[index] == 1)
Expand All @@ -562,8 +550,8 @@ namespace pcl
{
seeds.push(index);
}
}//next neighbour
}//next seed
}// next neighbour
}// next seed

return (static_cast<int>( out_new_segment.size() ));
}
Expand Down Expand Up @@ -598,13 +586,13 @@ namespace pcl
}
}

//check the curvature if needed
// check the curvature if needed
if (curvature_flag_ && normals_->points[nghbr].curvature > curvature_threshold_)
{
is_a_seed = false;
}

//check the residual if needed
// check the residual if needed
Eigen::Map<Eigen::Vector3f> nghbr_point( (float*)cloud_for_segmentation_->points[nghbr].data );
float residual = fabs( initial_normal.dot(initial_point - nghbr_point) );
if (residual_flag_ && residual > residual_threshold_)
Expand All @@ -618,4 +606,4 @@ namespace pcl

#define PCL_INSTANTIATE_RegionGrowing(T) template class pcl::RegionGrowing<T>;

#endif
#endif

0 comments on commit 2155a08

Please sign in to comment.