From 8e0b9b2b8caeb17d5579ef00243a628aa59432ad Mon Sep 17 00:00:00 2001 From: "Radu B. Rusu" Date: Mon, 27 Feb 2012 00:22:35 +0000 Subject: [PATCH] fixed compiler warnings + unit test git-svn-id: svn+ssh://svn.pointclouds.org/pcl/trunk@4776 a9d63959-f2ad-4865-b262-bf0e56cfafb6 --- .../pcl/segmentation/impl/region_growing.hpp | 890 +++++++++--------- .../include/pcl/segmentation/region_growing.h | 8 +- test/test_segmentation.cpp | 18 +- 3 files changed, 454 insertions(+), 462 deletions(-) diff --git a/segmentation/include/pcl/segmentation/impl/region_growing.hpp b/segmentation/include/pcl/segmentation/impl/region_growing.hpp index 4aa4a5afb68..8cd02987be0 100644 --- a/segmentation/include/pcl/segmentation/impl/region_growing.hpp +++ b/segmentation/include/pcl/segmentation/impl/region_growing.hpp @@ -37,572 +37,564 @@ * */ -#ifndef _REGIONGROWING_HPP_ -#define _REGIONGROWING_HPP_ +#ifndef PCL_SEGMENTATION_REGION_GROWING_HPP_ +#define PCL_SEGMENTATION_REGION_GROWING_HPP_ #include "pcl/segmentation/region_growing.h" -#include "pcl/search/search.h" -#include "pcl/point_cloud.h" -#include "pcl/point_types.h" +#include +#include +#include +#include +#include #include -#include +#include #include -namespace pcl +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::RegionGrowing::RegionGrowing () { - template - RegionGrowing::RegionGrowing () - { - theta_threshold_ = 30.0 / 180.0 * M_PI; - residual_threshold_ = 0.05; - curvature_threshold_ = 0.05; - neighbour_number_ = 30; - curvature_flag_ = true; - residual_flag_ = false; - smooth_mode_ = true; - segments_.clear(); - } + theta_threshold_ = 30.0 / 180.0 * M_PI; + residual_threshold_ = 0.05; + curvature_threshold_ = 0.05; + neighbour_number_ = 30; + curvature_flag_ = true; + residual_flag_ = false; + smooth_mode_ = true; + segments_.clear (); +} - template - RegionGrowing::~RegionGrowing () - { - if (normals_ != 0) - { - normals_.reset(); - } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::RegionGrowing::~RegionGrowing () +{ + if (normals_ != 0) + normals_.reset (); - if (search_ != 0) - { - search_.reset(); - } + if (search_ != 0) + search_.reset (); - segments_.clear(); + segments_.clear (); - if (cloud_for_segmentation_ != 0) - { - cloud_for_segmentation_.reset(); - } - } + if (cloud_for_segmentation_ != 0) + cloud_for_segmentation_.reset (); +} - template void - RegionGrowing::setSmoothMode (bool value) - { - smooth_mode_ = value; - } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::setSmoothMode (bool value) +{ + smooth_mode_ = value; +} - template void - RegionGrowing::setCurvatureTest (bool value) - { - curvature_flag_ = value; +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::setCurvatureTest (bool value) +{ + curvature_flag_ = value; - if (curvature_flag_ == false && residual_flag_ == false) - { - residual_flag_ = true; - } - } + if (curvature_flag_ == false && residual_flag_ == false) + residual_flag_ = true; +} - template void - RegionGrowing::setResidualTest (bool value) - { - residual_flag_ = value; +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::setResidualTest (bool value) +{ + residual_flag_ = value; - if (curvature_flag_ == false && residual_flag_ == false) - { - curvature_flag_ = true; - } - } + if (curvature_flag_ == false && residual_flag_ == false) + curvature_flag_ = true; +} - template bool - RegionGrowing::getSmoothModeFlag () const - { - return (smooth_mode_); - } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::RegionGrowing::getSmoothModeFlag () const +{ + return (smooth_mode_); +} - template bool - RegionGrowing::getCurvatureTestFlag () const - { - return (curvature_flag_); - } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::RegionGrowing::getCurvatureTestFlag () const +{ + return (curvature_flag_); +} - template bool - RegionGrowing::getResidualTestFlag () const - { - return (residual_flag_); - } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::RegionGrowing::getResidualTestFlag () const +{ + return (residual_flag_); +} - template float - RegionGrowing::getSmoothnessThreshold () const - { - return (theta_threshold_); - } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::RegionGrowing::getSmoothnessThreshold () const +{ + return (theta_threshold_); +} - template float - RegionGrowing::getResidualThreshold () const - { - return (residual_threshold_); - } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::RegionGrowing::getResidualThreshold () const +{ + return (residual_threshold_); +} - template float - RegionGrowing::getCurvatureThreshold () const - { - return (curvature_threshold_); - } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::RegionGrowing::getCurvatureThreshold () const +{ + return (curvature_threshold_); +} - template unsigned int - RegionGrowing::getNumberOfNeighbours () const - { - return (neighbour_number_); - } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template unsigned int +pcl::RegionGrowing::getNumberOfNeighbours () const +{ + return (neighbour_number_); +} - template typename pcl::search::Search::Ptr - RegionGrowing::getNeighbourSearchMethod () const - { - return (search_); - } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template typename pcl::search::Search::Ptr +pcl::RegionGrowing::getNeighbourSearchMethod () const +{ + return (search_); +} - template pcl::PointCloud::Ptr - RegionGrowing::getNormals () const - { - return (normals_); - } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template pcl::PointCloud::Ptr +pcl::RegionGrowing::getNormals () const +{ + return (normals_); +} - template typename pcl::PointCloud::Ptr - RegionGrowing::getCloud () const - { - return (cloud_for_segmentation_); - } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template typename pcl::PointCloud::Ptr +pcl::RegionGrowing::getCloud () const +{ + return (cloud_for_segmentation_); +} - template std::vector> - RegionGrowing::getSegments () const - { - return (segments_); - } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template std::vector > +pcl::RegionGrowing::getSegments () const +{ + return (segments_); +} - template std::list - RegionGrowing::getSegmentFromPoint (int index) - { - std::list result; - if (cloud_for_segmentation_ == 0) - { - return (result); - } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template std::list +pcl::RegionGrowing::getSegmentFromPoint (int index) +{ + std::list result; + if (cloud_for_segmentation_ == 0) + return (result); - // first of all we need to find out if this point belongs to cloud - bool point_was_found = false; - if (index < cloud_for_segmentation_->points.size() && index >= 0) - { - point_was_found = true; - } + // 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) + point_was_found = true; - if (point_was_found) + if (point_was_found) + { + if (!segments_.empty ()) { - if ( !segments_.empty() ) + // if we have already made the segmentation, then find the segment + // to which this point belongs + std::vector >::iterator i_segment; + for (i_segment = segments_.begin(); i_segment != segments_.end(); i_segment++) { - // if we have already made the segmentation, then find the segment - // to which this point belongs - std::vector>::iterator i_segment; - for (i_segment = segments_.begin(); i_segment != segments_.end(); i_segment++) + bool segment_was_found = false; + result.clear(); + result = *i_segment; + std::list::iterator i_point; + for (i_point = result.begin(); i_point != result.end(); i_point++) { - bool segment_was_found = false; - result.clear(); - result = *i_segment; - std::list::iterator i_point; - for (i_point = result.begin(); i_point != result.end(); i_point++) - { - if (*i_point == index) - { - segment_was_found = true; - break; - } - } - if (segment_was_found) + if (*i_point == index) { + segment_was_found = true; break; } - }// next segment - }// end if segments are not empty - else - { - bool segmentation_is_possible = prepareForSegmentation(); - if ( !segmentation_is_possible ) + } + if (segment_was_found) { - return (result); + break; } - // if we haven't done the segmentation yet, then we need to grow the segment - std::vector point_is_used; - point_is_used.resize(cloud_for_segmentation_->points.size(), 0); - growRegion(index, point_is_used, result); + }// next segment + }// end if segments are not empty + else + { + bool segmentation_is_possible = prepareForSegmentation(); + if ( !segmentation_is_possible ) + { + return (result); } - }// end if point was found + // if we haven't done the segmentation yet, then we need to grow the segment + std::vector point_is_used; + point_is_used.resize(cloud_for_segmentation_->points.size(), 0); + growRegion(index, point_is_used, result); + } + }// end if point was found + + return (result); +} +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template std::list +pcl::RegionGrowing::getSegmentFromPoint (const PointT &point) +{ + std::list result; + if (cloud_for_segmentation_ == 0) return (result); - } - template std::list - RegionGrowing::getSegmentFromPoint (typename PointT point) + // 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++) { - std::list result; - if (cloud_for_segmentation_ == 0) - { - return (result); - } + if (cloud_for_segmentation_->points[i].x != point.x) continue; + if (cloud_for_segmentation_->points[i].x != point.x) continue; + if (cloud_for_segmentation_->points[i].x != point.x) continue; - // first of all we need to find out if this point belongs to cloud - bool point_was_found = false; - int index = 0; - 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; - if (cloud_for_segmentation_->points[i].x != point.x) continue; - - point_was_found = true; - index = i; - break; - } + point_was_found = true; + index = i; + break; + } - if (point_was_found) + if (point_was_found) + { + if (!segments_.empty ()) { - if ( !segments_.empty() ) + // if we have already made the segmentation, then find the segment + // to which this point belongs + std::vector >::iterator i_segment; + for (i_segment = segments_.begin (); i_segment != segments_.end (); i_segment++) { - // if we have already made the segmentation, then find the segment - // to which this point belongs - std::vector>::iterator i_segment; - for (i_segment = segments_.begin(); i_segment != segments_.end(); i_segment++) + bool segment_was_found = false; + result.clear(); + result = *i_segment; + std::list::iterator i_point; + for (i_point = result.begin(); i_point != result.end(); i_point++) { - bool segment_was_found = false; - result.clear(); - result = *i_segment; - std::list::iterator i_point; - for (i_point = result.begin(); i_point != result.end(); i_point++) - { - if (*i_point == index) - { - segment_was_found = true; - break; - } - } - if (segment_was_found) + if (*i_point == index) { + segment_was_found = true; break; } - }// next segment - }// end if segments are not empty - else - { - bool segmentation_is_possible = prepareForSegmentation(); - if ( !segmentation_is_possible ) + } + if (segment_was_found) { - return (result); + break; } - // if we haven't done the segmentation yet, then we need to grow the segment - std::vector point_is_used; - point_is_used.resize(cloud_for_segmentation_->points.size(), 0); - growRegion(index, point_is_used, result); + }// next segment + }// end if segments are not empty + else + { + bool segmentation_is_possible = prepareForSegmentation(); + if ( !segmentation_is_possible ) + { + return (result); } - }// end if point was found + // if we haven't done the segmentation yet, then we need to grow the segment + std::vector point_is_used; + point_is_used.resize(cloud_for_segmentation_->points.size(), 0); + growRegion(index, point_is_used, result); + } + }// end if point was found - return (result); - } + return (result); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template pcl::PointCloud::Ptr +pcl::RegionGrowing::getColoredCloud () +{ + pcl::PointCloud::Ptr colored_cloud; - template pcl::PointCloud::Ptr - RegionGrowing::getColoredCloud () + if (!segments_.empty()) { - pcl::PointCloud::Ptr colored_cloud; + colored_cloud = (new pcl::PointCloud)->makeShared(); - if (!segments_.empty()) + srand(static_cast(time(0))); + std::vector colors; + for (size_t i_segment = 0; i_segment < segments_.size(); i_segment++) { - colored_cloud = (new pcl::PointCloud)->makeShared(); - - srand(static_cast(time(0))); - std::vector colors; - for (int i_segment = 0; i_segment < segments_.size(); i_segment++) - { - colors.push_back(rand() % 256); - colors.push_back(rand() % 256); - colors.push_back(rand() % 256); - } + colors.push_back(rand() % 256); + colors.push_back(rand() % 256); + colors.push_back(rand() % 256); + } - colored_cloud->width = cloud_for_segmentation_->width; - colored_cloud->height = cloud_for_segmentation_->height; - colored_cloud->is_dense = cloud_for_segmentation_->is_dense; - 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); - point.y = *(cloud_for_segmentation_->points[i_point].data + 1); - point.z = *(cloud_for_segmentation_->points[i_point].data + 2); - colored_cloud->points.push_back(point); - } + 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++) + { + pcl::PointXYZRGB point; + point.x = *(cloud_for_segmentation_->points[i_point].data); + point.y = *(cloud_for_segmentation_->points[i_point].data + 1); + point.z = *(cloud_for_segmentation_->points[i_point].data + 2); + colored_cloud->points.push_back(point); + } - std::vector>::iterator i_segment; - int next_color = 0; - for (i_segment = segments_.begin(); i_segment != segments_.end(); i_segment++) + std::vector >::iterator i_segment; + int next_color = 0; + for (i_segment = segments_.begin(); i_segment != segments_.end(); i_segment++) + { + std::list::iterator i_point; + for (i_point = i_segment->begin(); i_point != i_segment->end(); i_point++) { - std::list::iterator i_point; - for (i_point = i_segment->begin(); i_point != i_segment->end(); i_point++) - { - int index; - index = *i_point; - colored_cloud->points[index].r = colors[3 * next_color]; - colored_cloud->points[index].g = colors[3 * next_color + 1]; - colored_cloud->points[index].b = colors[3 * next_color + 2]; - } - next_color++; + int index; + index = *i_point; + colored_cloud->points[index].r = colors[3 * next_color]; + colored_cloud->points[index].g = colors[3 * next_color + 1]; + colored_cloud->points[index].b = colors[3 * next_color + 2]; } + next_color++; } - - return (colored_cloud); } - template void - RegionGrowing::setSmoothnessThreshold (float theta) - { - theta_threshold_ = theta; - } + return (colored_cloud); +} - template void - RegionGrowing::setResidualThreshold (float residual) - { - residual_threshold_ = residual; - } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::setSmoothnessThreshold (float theta) +{ + theta_threshold_ = theta; +} - template void - RegionGrowing::setCurvatureThreshold (float curvature) - { - curvature_threshold_ = curvature; - } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::setResidualThreshold (float residual) +{ + residual_threshold_ = residual; +} - template void - RegionGrowing::setNumberOfNeighbours (unsigned int neighbour_number) - { - neighbour_number_ = neighbour_number; - } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::setCurvatureThreshold (float curvature) +{ + curvature_threshold_ = curvature; +} - template void - RegionGrowing::setNeighbourSearchMethod (typename pcl::search::Search::Ptr search) - { - if (search_ != 0) - { - search_.reset(); - } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::setNumberOfNeighbours (unsigned int neighbour_number) +{ + neighbour_number_ = neighbour_number; +} - search_ = search; - } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::setNeighbourSearchMethod (typename pcl::search::Search::Ptr search) +{ + if (search_ != 0) + search_.reset (); - template void - RegionGrowing::setNormals (pcl::PointCloud::Ptr normals) - { - if (normals_ != 0) - { - normals_.reset(); - } + search_ = search; +} - normals_ = normals; - } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::setNormals (pcl::PointCloud::Ptr normals) +{ + if (normals_ != 0) + normals_.reset (); - template void - RegionGrowing::setCloud (typename pcl::PointCloud::Ptr input_cloud) - { - if (cloud_for_segmentation_ != 0) - { - cloud_for_segmentation_.reset(); - } + normals_ = normals; +} - cloud_for_segmentation_ = input_cloud; - } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::setCloud (typename pcl::PointCloud::Ptr input_cloud) +{ + if (cloud_for_segmentation_ != 0) + cloud_for_segmentation_.reset (); - template unsigned int - RegionGrowing::segmentPoints () - { - segments_.clear(); - unsigned int number_of_segments = 0; - number_of_segments = applySmoothRegionGrowingAlgorithm(); - return (number_of_segments); - } + cloud_for_segmentation_ = input_cloud; +} - template bool - RegionGrowing::prepareForSegmentation () - { - // if user forgot to pass points or normals - if ( cloud_for_segmentation_ == 0 || normals_ == 0 ) - { - return (false); - } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template unsigned int +pcl::RegionGrowing::segmentPoints () +{ + segments_.clear (); + unsigned int number_of_segments = 0; + number_of_segments = applySmoothRegionGrowingAlgorithm (); + return (number_of_segments); +} - // if number of points an number of normals are different - if (cloud_for_segmentation_->points.size() != normals_->points.size()) - { - return (false); - } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::RegionGrowing::prepareForSegmentation () +{ + // if user forgot to pass points or normals + if (cloud_for_segmentation_ == 0 || normals_ == 0) + return (false); - // if the cloud is empty - if ( cloud_for_segmentation_->points.size() == 0 || normals_->points.size() == 0 ) - { - return (false); - } + // if number of points an number of normals are different + if (cloud_for_segmentation_->points.size () != normals_->points.size ()) + return (false); - // if user passed wrong parameters - if ( neighbour_number_ == 0 || residual_threshold_ <= 0.0f ) - { - return (false); - } + // if the cloud is empty + if (cloud_for_segmentation_->points.size () == 0 || normals_->points.size () == 0) + return (false); - // if user didn't set search method - if (search_ == 0) - { - search_ = boost::shared_ptr< pcl::search::Search >(new pcl::search::KdTree); - } + // if user passed wrong parameters + if (neighbour_number_ == 0 || residual_threshold_ <= 0.0f) + return (false); - search_->setInputCloud(cloud_for_segmentation_); + // if user didn't set search method + if (search_ == 0) + search_ = boost::shared_ptr > (new pcl::search::KdTree); - return (true); - } + search_->setInputCloud (cloud_for_segmentation_); + + return (true); +} - template unsigned int - RegionGrowing::applySmoothRegionGrowingAlgorithm () +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template unsigned int +pcl::RegionGrowing::applySmoothRegionGrowingAlgorithm () +{ + bool segmentation_is_possible = prepareForSegmentation (); + if (!segmentation_is_possible) { - bool segmentation_is_possible = prepareForSegmentation(); - if ( !segmentation_is_possible ) - { - return (0); - } + return (0); + } - int num_of_pts = static_cast( cloud_for_segmentation_->points.size() ); + int num_of_pts = static_cast(cloud_for_segmentation_->points.size ()); - std::vector point_residual; - point_residual.resize(num_of_pts, -1.0); + std::vector point_residual; + point_residual.resize(num_of_pts, -1.0); - std::vector point_is_used; - point_is_used.resize(num_of_pts, 0); + std::vector point_is_used; + point_is_used.resize(num_of_pts, 0); - for (size_t i_point = 0; i_point < num_of_pts; i_point++) - { - point_residual[i_point] = normals_->points[i_point].curvature; - } - std::sort(point_residual.begin(), point_residual.end()); - int seed = 0; + for (int i_point = 0; i_point < num_of_pts; i_point++) + point_residual[i_point] = normals_->points[i_point].curvature; + std::sort(point_residual.begin(), point_residual.end()); + int seed = 0; - int segmented_pts_num = 0; - while ( segmented_pts_num < num_of_pts ) - { - int pts_in_segment; - std::list curr_segment; - pts_in_segment = growRegion(seed, point_is_used, curr_segment); - segments_.push_back(curr_segment); - segmented_pts_num += pts_in_segment; + int segmented_pts_num = 0; + while ( segmented_pts_num < num_of_pts ) + { + int pts_in_segment; + std::list curr_segment; + pts_in_segment = growRegion(seed, point_is_used, curr_segment); + segments_.push_back(curr_segment); + segmented_pts_num += pts_in_segment; - for (int i_seed = seed + 1; i_seed < num_of_pts; i_seed++) + for (int i_seed = seed + 1; i_seed < num_of_pts; i_seed++) + { + if (point_is_used[i_seed] == 0) { - if (point_is_used[i_seed] == 0) - { - seed = i_seed; - break; - } + seed = i_seed; + break; } } - - return (static_cast( segments_.size() )); } - template int - RegionGrowing::growRegion (int initial_seed, std::vector& point_is_used, std::list& out_new_segment) + return (static_cast( segments_.size() )); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::RegionGrowing::growRegion (int initial_seed, std::vector& point_is_used, std::list& out_new_segment) +{ + std::queue seeds; + seeds.push(initial_seed); + out_new_segment.push_back(initial_seed); + + point_is_used[initial_seed] = 1; + + while (!seeds.empty()) { - std::queue seeds; - seeds.push(initial_seed); - out_new_segment.push_back(initial_seed); + int curr_seed; + curr_seed = seeds.front(); + seeds.pop(); - point_is_used[initial_seed] = 1; + std::vector nghbr_indices; + std::vector nghbr_distances; + search_->nearestKSearch(curr_seed, neighbour_number_, nghbr_indices, nghbr_distances); - while (!seeds.empty()) + for (size_t i_nghbr = 0; i_nghbr < nghbr_indices.size(); i_nghbr++) { - int curr_seed; - curr_seed = seeds.front(); - seeds.pop(); + int index = nghbr_indices[i_nghbr]; + if (point_is_used[index] == 1) + { + continue; + } - std::vector nghbr_indices; - std::vector nghbr_distances; - search_->nearestKSearch(curr_seed, neighbour_number_, nghbr_indices, nghbr_distances); + bool is_a_seed = false; + bool belongs_to_segment = validatePoint(initial_seed, curr_seed, index, is_a_seed); - for (int i_nghbr = 0; i_nghbr < nghbr_indices.size(); i_nghbr++) + if (belongs_to_segment == false) { - int index = nghbr_indices[i_nghbr]; - if (point_is_used[index] == 1) - { - continue; - } + continue; + } - bool is_a_seed = false; - bool belongs_to_segment = validatePoint(initial_seed, curr_seed, index, is_a_seed); + point_is_used[index] = 1; + out_new_segment.push_back(index); - if (belongs_to_segment == false) - { - continue; - } + if (is_a_seed) + { + seeds.push(index); + } + }// next neighbour + }// next seed - point_is_used[index] = 1; - out_new_segment.push_back(index); + return (static_cast( out_new_segment.size() )); +} - if (is_a_seed) - { - seeds.push(index); - } - }// next neighbour - }// next seed +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::RegionGrowing::validatePoint (int initial_seed, int point, int nghbr, bool& is_a_seed) const +{ + is_a_seed = true; - return (static_cast( out_new_segment.size() )); - } + float cosine_threshold = cos(theta_threshold_); + Eigen::Map initial_point( (float*)cloud_for_segmentation_->points[point].data ); + Eigen::Map initial_normal( (float*)normals_->points[point].normal ); - template bool - RegionGrowing::validatePoint (int initial_seed, int point, int nghbr, bool& is_a_seed) const + //check the angle between normals + if (smooth_mode_ == true) { - is_a_seed = true; - - float cosine_threshold = cos(theta_threshold_); - Eigen::Map initial_point( (float*)cloud_for_segmentation_->points[point].data ); - Eigen::Map initial_normal( (float*)normals_->points[point].normal ); - - //check the angle between normals - if (smooth_mode_ == true) - { - Eigen::Map nghbr_normal( (float*)normals_->points[nghbr].normal ); - float dot_product = fabs( nghbr_normal.dot(initial_normal) ); - if (dot_product < cosine_threshold) - { - return (false); - } - } - else + Eigen::Map nghbr_normal( (float*)normals_->points[nghbr].normal ); + float dot_product = fabs( nghbr_normal.dot(initial_normal) ); + if (dot_product < cosine_threshold) { - Eigen::Map nghbr_normal( (float*)normals_->points[nghbr].normal ); - Eigen::Map initial_seed_normal( (float*)normals_->points[initial_seed].normal ); - float dot_product = fabs( nghbr_normal.dot(initial_seed_normal) ); - if (dot_product < cosine_threshold) - { - return (false); - } + return (false); } + } + else + { + Eigen::Map nghbr_normal( (float*)normals_->points[nghbr].normal ); + Eigen::Map initial_seed_normal( (float*)normals_->points[initial_seed].normal ); + float dot_product = fabs( nghbr_normal.dot(initial_seed_normal) ); + if (dot_product < cosine_threshold) + return (false); + } - // check the curvature if needed - if (curvature_flag_ && normals_->points[nghbr].curvature > curvature_threshold_) - { - is_a_seed = false; - } + // check the curvature if needed + if (curvature_flag_ && normals_->points[nghbr].curvature > curvature_threshold_) + { + is_a_seed = false; + } - // check the residual if needed - Eigen::Map 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_) - { - is_a_seed = false; - } + // check the residual if needed + Eigen::Map 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_) + is_a_seed = false; - return (true); - } + return (true); } #define PCL_INSTANTIATE_RegionGrowing(T) template class pcl::RegionGrowing; -#endif \ No newline at end of file +#endif // PCL_SEGMENTATION_REGION_GROWING_HPP_ diff --git a/segmentation/include/pcl/segmentation/region_growing.h b/segmentation/include/pcl/segmentation/region_growing.h index bffd2770261..f6b83d4e32e 100644 --- a/segmentation/include/pcl/segmentation/region_growing.h +++ b/segmentation/include/pcl/segmentation/region_growing.h @@ -110,7 +110,7 @@ namespace pcl getCloud () const; /** \brief Returns list of segments. Each segment is a list of indices of points. */ - std::vector> + std::vector > getSegments () const; /** \brief This function allows to turn on/off the smoothness constraint. @@ -195,7 +195,7 @@ namespace pcl * \param[in] initial point which will be the seed for growing a segment. */ virtual std::list - getSegmentFromPoint (typename PointT point); + getSegmentFromPoint (const PointT &point); /** \brief For a given point this function builds a segment to which it belongs and returns this segment. * \param[in] index of the initial point which will be the seed for growing a segment. @@ -278,8 +278,8 @@ namespace pcl typename pcl::PointCloud::Ptr cloud_for_segmentation_; /** \brief After the segmentation it will contain the list of segments, which in turn are lists of indices. */ - std::vector> segments_; + std::vector > segments_; }; } -#endif \ No newline at end of file +#endif diff --git a/test/test_segmentation.cpp b/test/test_segmentation.cpp index e4d106f1b69..3bfc4461859 100644 --- a/test/test_segmentation.cpp +++ b/test/test_segmentation.cpp @@ -61,7 +61,7 @@ pcl::PointCloud::Ptr normals_; pcl::PointCloud::Ptr another_normals_; //////////////////////////////////////////////////////////////////////////////////////////////// -TEST (//gionGrowingTest, Segment) +TEST (RegionGrowingTest, Segment) { pcl::RegionGrowing rg; rg.setCloud(cloud_); @@ -70,7 +70,7 @@ TEST (//gionGrowingTest, Segment) int num_of_segments = rg.segmentPoints(); EXPECT_NE(0, num_of_segments); - std::vector> segments; + std::vector > segments; segments = rg.getSegments(); EXPECT_NE(0, segments.size()); } @@ -84,7 +84,7 @@ TEST (RegionGrowingTest, SegmentWithoutCloud) int num_of_segments = rg.segmentPoints(); EXPECT_EQ(0, num_of_segments); - std::vector> segments; + std::vector > segments; segments = rg.getSegments(); EXPECT_EQ(0, segments.size()); } @@ -98,7 +98,7 @@ TEST (RegionGrowingTest, SegmentWithoutNormals) int num_of_segments = rg.segmentPoints(); EXPECT_EQ(0, num_of_segments); - std::vector> segments; + std::vector > segments; segments = rg.getSegments(); EXPECT_EQ(0, segments.size()); } @@ -116,7 +116,7 @@ TEST (RegionGrowingTest, SegmentEmptyCloud) int num_of_segments = rg.segmentPoints(); EXPECT_EQ(0, num_of_segments); - std::vector> segments; + std::vector > segments; segments = rg.getSegments(); EXPECT_EQ(0, segments.size()); } @@ -135,7 +135,7 @@ TEST (RegionGrowingTest, SegmentWithDifferentNormalAndCloudSize) int num_of_segments = rg.segmentPoints(); EXPECT_EQ(0, num_of_segments); - std::vector> segments; + std::vector > segments; segments = rg.getSegments(); EXPECT_EQ(0, segments.size()); @@ -161,7 +161,7 @@ TEST (RegionGrowingTest, SegmentWithWrongThresholdParameters) int num_of_segments = rg.segmentPoints(); EXPECT_EQ(0, num_of_segments); - std::vector> segments; + std::vector > segments; segments = rg.getSegments(); EXPECT_EQ(0, segments.size()); @@ -176,7 +176,7 @@ TEST (RegionGrowingTest, SegmentWithWrongThresholdParameters) } //////////////////////////////////////////////////////////////////////////////////////////////// -//TEST (RegionGrowingTest, SegmentFromPoint) +TEST (RegionGrowingTest, SegmentFromPoint) { pcl::RegionGrowing rg; @@ -242,7 +242,7 @@ TEST (ExtractPolygonalPrism, Segmentation) /* ---[ */ int - main (int argc, char** argv) +main (int argc, char** argv) { if (argc < 3) {