region_growing_rgb.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* 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 PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_
#define PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_
#include <pcl/console/print.h> // for PCL_ERROR
#include <pcl/segmentation/region_growing_rgb.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <queue>
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename NormalT>
pcl::RegionGrowingRGB<PointT, NormalT>::RegionGrowingRGB () :
color_p2p_threshold_ (1225.0f),
color_r2r_threshold_ (10.0f),
distance_threshold_ (0.05f),
region_neighbour_number_ (100),
point_distances_ (0),
segment_neighbours_ (0),
segment_distances_ (0),
segment_labels_ (0)
{
normal_flag_ = false;
curvature_flag_ = false;
residual_flag_ = false;
min_pts_per_cluster_ = 10;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename NormalT>
pcl::RegionGrowingRGB<PointT, NormalT>::~RegionGrowingRGB ()
{
point_distances_.clear ();
segment_neighbours_.clear ();
segment_distances_.clear ();
segment_labels_.clear ();
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename NormalT> float
pcl::RegionGrowingRGB<PointT, NormalT>::getPointColorThreshold () const
{
return (powf (color_p2p_threshold_, 0.5f));
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename NormalT> void
pcl::RegionGrowingRGB<PointT, NormalT>::setPointColorThreshold (float thresh)
{
color_p2p_threshold_ = thresh * thresh;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename NormalT> float
pcl::RegionGrowingRGB<PointT, NormalT>::getRegionColorThreshold () const
{
return (powf (color_r2r_threshold_, 0.5f));
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename NormalT> void
pcl::RegionGrowingRGB<PointT, NormalT>::setRegionColorThreshold (float thresh)
{
color_r2r_threshold_ = thresh * thresh;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename NormalT> float
pcl::RegionGrowingRGB<PointT, NormalT>::getDistanceThreshold () const
{
return (powf (distance_threshold_, 0.5f));
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename NormalT> void
pcl::RegionGrowingRGB<PointT, NormalT>::setDistanceThreshold (float thresh)
{
distance_threshold_ = thresh * thresh;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename NormalT> unsigned int
pcl::RegionGrowingRGB<PointT, NormalT>::getNumberOfRegionNeighbours () const
{
return (region_neighbour_number_);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename NormalT> void
pcl::RegionGrowingRGB<PointT, NormalT>::setNumberOfRegionNeighbours (unsigned int nghbr_number)
{
region_neighbour_number_ = nghbr_number;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename NormalT> bool
pcl::RegionGrowingRGB<PointT, NormalT>::getNormalTestFlag () const
{
return (normal_flag_);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename NormalT> void
pcl::RegionGrowingRGB<PointT, NormalT>::setNormalTestFlag (bool value)
{
normal_flag_ = value;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename NormalT> void
pcl::RegionGrowingRGB<PointT, NormalT>::setCurvatureTestFlag (bool value)
{
curvature_flag_ = value;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename NormalT> void
pcl::RegionGrowingRGB<PointT, NormalT>::setResidualTestFlag (bool value)
{
residual_flag_ = value;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename NormalT> void
pcl::RegionGrowingRGB<PointT, NormalT>::extract (std::vector <pcl::PointIndices>& clusters)
{
clusters_.clear ();
clusters.clear ();
point_neighbours_.clear ();
point_labels_.clear ();
num_pts_in_segment_.clear ();
point_distances_.clear ();
segment_neighbours_.clear ();
segment_distances_.clear ();
segment_labels_.clear ();
number_of_segments_ = 0;
bool segmentation_is_possible = initCompute ();
if ( !segmentation_is_possible )
{
deinitCompute ();
return;
}
segmentation_is_possible = prepareForSegmentation ();
if ( !segmentation_is_possible )
{
deinitCompute ();
return;
}
findPointNeighbours ();
applySmoothRegionGrowingAlgorithm ();
RegionGrowing<PointT, NormalT>::assembleRegions ();
findSegmentNeighbours ();
applyRegionMergingAlgorithm ();
std::vector<pcl::PointIndices>::iterator cluster_iter = clusters_.begin ();
while (cluster_iter != clusters_.end ())
{
if (cluster_iter->indices.size () < min_pts_per_cluster_ ||
cluster_iter->indices.size () > max_pts_per_cluster_)
{
cluster_iter = clusters_.erase (cluster_iter);
}
else
++cluster_iter;
}
clusters.reserve (clusters_.size ());
std::copy (clusters_.begin (), clusters_.end (), std::back_inserter (clusters));
deinitCompute ();
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename NormalT> bool
pcl::RegionGrowingRGB<PointT, NormalT>::prepareForSegmentation ()
{
// if user forgot to pass point cloud or if it is empty
if ( input_->points.empty () )
return (false);
// if normal/smoothness test is on then we need to check if all needed variables and parameters
// were correctly initialized
if (normal_flag_)
{
// if user forgot to pass normals or the sizes of point and normal cloud are different
if ( !normals_ || input_->size () != normals_->size () )
return (false);
}
// if residual test is on then we need to check if all needed parameters were correctly initialized
if (residual_flag_)
{
if (residual_threshold_ <= 0.0f)
return (false);
}
// if curvature test is on ...
// if (curvature_flag_)
// {
// in this case we do not need to check anything that related to it
// so we simply commented it
// }
// here we check the parameters related to color-based segmentation
if ( region_neighbour_number_ == 0 || color_p2p_threshold_ < 0.0f || color_r2r_threshold_ < 0.0f || distance_threshold_ < 0.0f )
return (false);
// from here we check those parameters that are always valuable
if (neighbour_number_ == 0)
return (false);
// if user didn't set search method
if (!search_)
search_.reset (new pcl::search::KdTree<PointT>);
if (indices_)
{
if (indices_->empty ())
PCL_ERROR ("[pcl::RegionGrowingRGB::prepareForSegmentation] Empty given indices!\n");
search_->setInputCloud (input_, indices_);
}
else
search_->setInputCloud (input_);
return (true);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename NormalT> void
pcl::RegionGrowingRGB<PointT, NormalT>::findPointNeighbours ()
{
int point_number = static_cast<int> (indices_->size ());
pcl::Indices neighbours;
std::vector<float> distances;
point_neighbours_.resize (input_->size (), neighbours);
point_distances_.resize (input_->size (), distances);
for (int i_point = 0; i_point < point_number; i_point++)
{
int point_index = (*indices_)[i_point];
neighbours.clear ();
distances.clear ();
search_->nearestKSearch (i_point, region_neighbour_number_, neighbours, distances);
point_neighbours_[point_index].swap (neighbours);
point_distances_[point_index].swap (distances);
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename NormalT> void
pcl::RegionGrowingRGB<PointT, NormalT>::findSegmentNeighbours ()
{
pcl::Indices neighbours;
std::vector<float> distances;
segment_neighbours_.resize (number_of_segments_, neighbours);
segment_distances_.resize (number_of_segments_, distances);
for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
{
pcl::Indices nghbrs;
std::vector<float> dist;
findRegionsKNN (i_seg, region_neighbour_number_, nghbrs, dist);
segment_neighbours_[i_seg].swap (nghbrs);
segment_distances_[i_seg].swap (dist);
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT,typename NormalT> void
pcl::RegionGrowingRGB<PointT, NormalT>::findRegionsKNN (pcl::index_t index, pcl::uindex_t nghbr_number, pcl::Indices& nghbrs, std::vector<float>& dist)
{
std::vector<float> distances;
float max_dist = std::numeric_limits<float>::max ();
distances.resize (clusters_.size (), max_dist);
const auto number_of_points = num_pts_in_segment_[index];
//loop through every point in this segment and check neighbours
for (pcl::uindex_t i_point = 0; i_point < number_of_points; i_point++)
{
const auto point_index = clusters_[index].indices[i_point];
const auto number_of_neighbours = point_neighbours_[point_index].size ();
//loop through every neighbour of the current point, find out to which segment it belongs
//and if it belongs to neighbouring segment and is close enough then remember segment and its distance
for (std::size_t i_nghbr = 0; i_nghbr < number_of_neighbours; i_nghbr++)
{
// find segment
const pcl::index_t segment_index = point_labels_[ point_neighbours_[point_index][i_nghbr] ];
if ( segment_index != index )
{
// try to push it to the queue
if (distances[segment_index] > point_distances_[point_index][i_nghbr])
distances[segment_index] = point_distances_[point_index][i_nghbr];
}
}
}// next point
std::priority_queue<std::pair<float, int> > segment_neighbours;
for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
{
if (distances[i_seg] < max_dist)
{
segment_neighbours.push (std::make_pair (distances[i_seg], i_seg) );
if (segment_neighbours.size () > nghbr_number)
segment_neighbours.pop ();
}
}
const std::size_t size = std::min<std::size_t> (segment_neighbours.size (), static_cast<std::size_t>(nghbr_number));
nghbrs.resize (size, 0);
dist.resize (size, 0);
pcl::uindex_t counter = 0;
while ( !segment_neighbours.empty () && counter < nghbr_number )
{
dist[counter] = segment_neighbours.top ().first;
nghbrs[counter] = segment_neighbours.top ().second;
segment_neighbours.pop ();
counter++;
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename NormalT> void
pcl::RegionGrowingRGB<PointT, NormalT>::applyRegionMergingAlgorithm ()
{
// calculate color of each segment
std::vector< std::vector<unsigned int> > segment_color;
std::vector<unsigned int> color;
color.resize (3, 0);
segment_color.resize (number_of_segments_, color);
for (const auto& point_index : (*indices_))
{
int segment_index = point_labels_[point_index];
segment_color[segment_index][0] += (*input_)[point_index].r;
segment_color[segment_index][1] += (*input_)[point_index].g;
segment_color[segment_index][2] += (*input_)[point_index].b;
}
for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
{
segment_color[i_seg][0] = static_cast<unsigned int> (static_cast<float> (segment_color[i_seg][0]) / static_cast<float> (num_pts_in_segment_[i_seg]));
segment_color[i_seg][1] = static_cast<unsigned int> (static_cast<float> (segment_color[i_seg][1]) / static_cast<float> (num_pts_in_segment_[i_seg]));
segment_color[i_seg][2] = static_cast<unsigned int> (static_cast<float> (segment_color[i_seg][2]) / static_cast<float> (num_pts_in_segment_[i_seg]));
}
// now it is time to find out if there are segments with a similar color
// and merge them together
std::vector<unsigned int> num_pts_in_homogeneous_region;
std::vector<int> num_seg_in_homogeneous_region;
segment_labels_.resize (number_of_segments_, -1);
float dist_thresh = distance_threshold_;
int homogeneous_region_number = 0;
for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
{
int curr_homogeneous_region = 0;
if (segment_labels_[i_seg] == -1)
{
segment_labels_[i_seg] = homogeneous_region_number;
curr_homogeneous_region = homogeneous_region_number;
num_pts_in_homogeneous_region.push_back (num_pts_in_segment_[i_seg]);
num_seg_in_homogeneous_region.push_back (1);
homogeneous_region_number++;
}
else
curr_homogeneous_region = segment_labels_[i_seg];
unsigned int i_nghbr = 0;
while ( i_nghbr < region_neighbour_number_ && i_nghbr < segment_neighbours_[i_seg].size () )
{
int index = segment_neighbours_[i_seg][i_nghbr];
if (segment_distances_[i_seg][i_nghbr] > dist_thresh)
{
i_nghbr++;
continue;
}
if ( segment_labels_[index] == -1 )
{
float difference = calculateColorimetricalDifference (segment_color[i_seg], segment_color[index]);
if (difference < color_r2r_threshold_)
{
segment_labels_[index] = curr_homogeneous_region;
num_pts_in_homogeneous_region[curr_homogeneous_region] += num_pts_in_segment_[index];
num_seg_in_homogeneous_region[curr_homogeneous_region] += 1;
}
}
i_nghbr++;
}// next neighbour
}// next segment
segment_color.clear ();
color.clear ();
std::vector< std::vector<int> > final_segments;
std::vector<int> region;
final_segments.resize (homogeneous_region_number, region);
for (int i_reg = 0; i_reg < homogeneous_region_number; i_reg++)
{
final_segments[i_reg].resize (num_seg_in_homogeneous_region[i_reg], 0);
}
std::vector<int> counter;
counter.resize (homogeneous_region_number, 0);
for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
{
int index = segment_labels_[i_seg];
final_segments[ index ][ counter[index] ] = i_seg;
counter[index] += 1;
}
std::vector< std::vector< std::pair<float, pcl::index_t> > > region_neighbours;
findRegionNeighbours (region_neighbours, final_segments);
int final_segment_number = homogeneous_region_number;
for (int i_reg = 0; i_reg < homogeneous_region_number; i_reg++)
{
if (num_pts_in_homogeneous_region[i_reg] < min_pts_per_cluster_)
{
if ( region_neighbours[i_reg].empty () )
continue;
int nearest_neighbour = region_neighbours[i_reg][0].second;
if ( region_neighbours[i_reg][0].first == std::numeric_limits<float>::max () )
continue;
int reg_index = segment_labels_[nearest_neighbour];
int num_seg_in_reg = num_seg_in_homogeneous_region[i_reg];
for (int i_seg = 0; i_seg < num_seg_in_reg; i_seg++)
{
int segment_index = final_segments[i_reg][i_seg];
final_segments[reg_index].push_back (segment_index);
segment_labels_[segment_index] = reg_index;
}
final_segments[i_reg].clear ();
num_pts_in_homogeneous_region[reg_index] += num_pts_in_homogeneous_region[i_reg];
num_pts_in_homogeneous_region[i_reg] = 0;
num_seg_in_homogeneous_region[reg_index] += num_seg_in_homogeneous_region[i_reg];
num_seg_in_homogeneous_region[i_reg] = 0;
final_segment_number -= 1;
for (auto& nghbr : region_neighbours[reg_index])
{
if ( segment_labels_[ nghbr.second ] == reg_index )
{
nghbr.first = std::numeric_limits<float>::max ();
nghbr.second = 0;
}
}
for (const auto& nghbr : region_neighbours[i_reg])
{
if ( segment_labels_[ nghbr.second ] != reg_index )
{
region_neighbours[reg_index].push_back (nghbr);
}
}
region_neighbours[i_reg].clear ();
std::sort (region_neighbours[reg_index].begin (), region_neighbours[reg_index].end (), comparePair);
}
}
assembleRegions (num_pts_in_homogeneous_region, static_cast<int> (num_pts_in_homogeneous_region.size ()));
number_of_segments_ = final_segment_number;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename NormalT> float
pcl::RegionGrowingRGB<PointT, NormalT>::calculateColorimetricalDifference (std::vector<unsigned int>& first_color, std::vector<unsigned int>& second_color) const
{
float difference = 0.0f;
difference += float ((first_color[0] - second_color[0]) * (first_color[0] - second_color[0]));
difference += float ((first_color[1] - second_color[1]) * (first_color[1] - second_color[1]));
difference += float ((first_color[2] - second_color[2]) * (first_color[2] - second_color[2]));
return (difference);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename NormalT> void
pcl::RegionGrowingRGB<PointT, NormalT>::findRegionNeighbours (std::vector< std::vector< std::pair<float, pcl::index_t> > >& neighbours_out, std::vector< std::vector<int> >& regions_in)
{
int region_number = static_cast<int> (regions_in.size ());
neighbours_out.clear ();
neighbours_out.resize (region_number);
for (int i_reg = 0; i_reg < region_number; i_reg++)
{
neighbours_out[i_reg].reserve (regions_in[i_reg].size () * region_neighbour_number_);
for (const auto& curr_segment : regions_in[i_reg])
{
const std::size_t nghbr_number = segment_neighbours_[curr_segment].size ();
std::pair<float, pcl::index_t> pair;
for (std::size_t i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
{
const auto segment_index = segment_neighbours_[curr_segment][i_nghbr];
if ( segment_distances_[curr_segment][i_nghbr] == std::numeric_limits<float>::max () )
continue;
if (segment_labels_[segment_index] != i_reg)
{
pair.first = segment_distances_[curr_segment][i_nghbr];
pair.second = segment_index;
neighbours_out[i_reg].push_back (pair);
}
}// next neighbour
}// next segment
std::sort (neighbours_out[i_reg].begin (), neighbours_out[i_reg].end (), comparePair);
}// next homogeneous region
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename NormalT> void
pcl::RegionGrowingRGB<PointT, NormalT>::assembleRegions (std::vector<unsigned int>& num_pts_in_region, int num_regions)
{
clusters_.clear ();
pcl::PointIndices segment;
clusters_.resize (num_regions, segment);
for (int i_seg = 0; i_seg < num_regions; i_seg++)
{
clusters_[i_seg].indices.resize (num_pts_in_region[i_seg]);
}
std::vector<int> counter;
counter.resize (num_regions, 0);
for (const auto& point_index : (*indices_))
{
int index = point_labels_[point_index];
index = segment_labels_[index];
clusters_[index].indices[ counter[index] ] = point_index;
counter[index] += 1;
}
// now we need to erase empty regions
if (clusters_.empty ())
return;
std::vector<pcl::PointIndices>::iterator itr1, itr2;
itr1 = clusters_.begin ();
itr2 = clusters_.end () - 1;
while (itr1 < itr2)
{
while (!(itr1->indices.empty ()) && itr1 < itr2)
++itr1;
while ( itr2->indices.empty () && itr1 < itr2)
--itr2;
if (itr1 != itr2)
itr1->indices.swap (itr2->indices);
}
if (itr2->indices.empty ())
clusters_.erase (itr2, clusters_.end ());
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename NormalT> bool
pcl::RegionGrowingRGB<PointT, NormalT>::validatePoint (pcl::index_t initial_seed, pcl::index_t point, pcl::index_t nghbr, bool& is_a_seed) const
{
is_a_seed = true;
// check the color difference
std::vector<unsigned int> point_color;
point_color.resize (3, 0);
std::vector<unsigned int> nghbr_color;
nghbr_color.resize (3, 0);
point_color[0] = (*input_)[point].r;
point_color[1] = (*input_)[point].g;
point_color[2] = (*input_)[point].b;
nghbr_color[0] = (*input_)[nghbr].r;
nghbr_color[1] = (*input_)[nghbr].g;
nghbr_color[2] = (*input_)[nghbr].b;
float difference = calculateColorimetricalDifference (point_color, nghbr_color);
if (difference > color_p2p_threshold_)
return (false);
float cosine_threshold = std::cos (theta_threshold_);
// check the angle between normals if needed
if (normal_flag_)
{
float data[4];
data[0] = (*input_)[point].data[0];
data[1] = (*input_)[point].data[1];
data[2] = (*input_)[point].data[2];
data[3] = (*input_)[point].data[3];
Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data));
Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> ((*normals_)[point].normal));
if (smooth_mode_flag_ == true)
{
Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> ((*normals_)[nghbr].normal));
float dot_product = std::abs (nghbr_normal.dot (initial_normal));
if (dot_product < cosine_threshold)
return (false);
}
else
{
Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> ((*normals_)[nghbr].normal));
Eigen::Map<Eigen::Vector3f> initial_seed_normal (static_cast<float*> ((*normals_)[initial_seed].normal));
float dot_product = std::abs (nghbr_normal.dot (initial_seed_normal));
if (dot_product < cosine_threshold)
return (false);
}
}
// check the curvature if needed
if (curvature_flag_ && (*normals_)[nghbr].curvature > curvature_threshold_)
is_a_seed = false;
// check the residual if needed
if (residual_flag_)
{
float data_p[4];
data_p[0] = (*input_)[point].data[0];
data_p[1] = (*input_)[point].data[1];
data_p[2] = (*input_)[point].data[2];
data_p[3] = (*input_)[point].data[3];
float data_n[4];
data_n[0] = (*input_)[nghbr].data[0];
data_n[1] = (*input_)[nghbr].data[1];
data_n[2] = (*input_)[nghbr].data[2];
data_n[3] = (*input_)[nghbr].data[3];
Eigen::Map<Eigen::Vector3f> nghbr_point (static_cast<float*> (data_n));
Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data_p));
Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> ((*normals_)[point].normal));
float residual = std::abs (initial_normal.dot (initial_point - nghbr_point));
if (residual > residual_threshold_)
is_a_seed = false;
}
return (true);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename NormalT> void
pcl::RegionGrowingRGB<PointT, NormalT>::getSegmentFromPoint (pcl::index_t index, pcl::PointIndices& cluster)
{
cluster.indices.clear ();
bool segmentation_is_possible = initCompute ();
if ( !segmentation_is_possible )
{
deinitCompute ();
return;
}
// first of all we need to find out if this point belongs to cloud
bool point_was_found = false;
for (const auto& point : (*indices_))
if (point == index)
{
point_was_found = true;
break;
}
if (point_was_found)
{
if (clusters_.empty ())
{
clusters_.clear ();
point_neighbours_.clear ();
point_labels_.clear ();
num_pts_in_segment_.clear ();
point_distances_.clear ();
segment_neighbours_.clear ();
segment_distances_.clear ();
segment_labels_.clear ();
number_of_segments_ = 0;
segmentation_is_possible = prepareForSegmentation ();
if ( !segmentation_is_possible )
{
deinitCompute ();
return;
}
findPointNeighbours ();
applySmoothRegionGrowingAlgorithm ();
RegionGrowing<PointT, NormalT>::assembleRegions ();
findSegmentNeighbours ();
applyRegionMergingAlgorithm ();
}
// if we have already made the segmentation, then find the segment
// to which this point belongs
for (const auto& i_segment : clusters_)
{
const auto it = std::find (i_segment.indices.cbegin (), i_segment.indices.cend (), index);
if (it != i_segment.indices.cend())
{
// if segment was found
cluster.indices.clear ();
cluster.indices.reserve (i_segment.indices.size ());
std::copy (i_segment.indices.begin (), i_segment.indices.end (), std::back_inserter (cluster.indices));
break;
}
}// next segment
}// end if point was found
deinitCompute ();
}
#endif // PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_
pcl::RegionGrowing< PointT, pcl::Normal >::curvature_flag_
bool curvature_flag_
If set to true then curvature test will be done during segmentation.
Definition: region_growing.h:291
pcl::RegionGrowing< PointT, pcl::Normal >::normal_flag_
bool normal_flag_
If set to true then normal/smoothness test will be done during segmentation.
Definition: region_growing.h:323
pcl::RegionGrowing< PointT, pcl::Normal >::residual_flag_
bool residual_flag_
If set to true then residual test will be done during segmentation.
Definition: region_growing.h:294
pcl::RegionGrowing< PointT, pcl::Normal >::min_pts_per_cluster_
pcl::uindex_t min_pts_per_cluster_
Stores the minimum number of points that a cluster needs to contain in order to be considered valid.
Definition: region_growing.h:282
pcl::RegionGrowing::assembleRegions
void assembleRegions()
This function simply assembles the regions from list of point labels.
Definition: region_growing.hpp:539
pcl::RegionGrowingRGB::getSegmentFromPoint
void getSegmentFromPoint(index_t index, pcl::PointIndices &cluster) override
For a given point this function builds a segment to which it belongs and returns this segment.
Definition: region_growing_rgb.hpp:678
pcl::RegionGrowingRGB::setDistanceThreshold
void setDistanceThreshold(float thresh)
Allows to set distance threshold.
Definition: region_growing_rgb.hpp:115
pcl::RegionGrowingRGB::setRegionColorThreshold
void setRegionColorThreshold(float thresh)
This method specifies the threshold value for color test between the regions.
Definition: region_growing_rgb.hpp:101
pcl::RegionGrowingRGB::getPointColorThreshold
float getPointColorThreshold() const
Returns the color threshold value used for testing if points belong to the same region.
Definition: region_growing_rgb.hpp:80
pcl::RegionGrowingRGB::prepareForSegmentation
bool prepareForSegmentation() override
This method simply checks if it is possible to execute the segmentation algorithm with the current se...
Definition: region_growing_rgb.hpp:218
pcl::RegionGrowingRGB::findRegionNeighbours
void findRegionNeighbours(std::vector< std::vector< std::pair< float, pcl::index_t > > > &neighbours_out, std::vector< std::vector< int > > &regions_in)
This method assembles the array containing neighbours of each homogeneous region.
Definition: region_growing_rgb.hpp:522
pcl::RegionGrowingRGB::applyRegionMergingAlgorithm
void applyRegionMergingAlgorithm()
This function implements the merging algorithm described in the article "Color-based segmentation of ...
Definition: region_growing_rgb.hpp:368
pcl::RegionGrowingRGB::getNumberOfRegionNeighbours
unsigned int getNumberOfRegionNeighbours() const
Returns the number of nearest neighbours used for searching K nearest segments.
Definition: region_growing_rgb.hpp:122
pcl::RegionGrowingRGB::setCurvatureTestFlag
void setCurvatureTestFlag(bool value) override
Allows to turn on/off the curvature test.
Definition: region_growing_rgb.hpp:150
pcl::RegionGrowingRGB::extract
void extract(std::vector< pcl::PointIndices > &clusters) override
This method launches the segmentation algorithm and returns the clusters that were obtained during th...
Definition: region_growing_rgb.hpp:164
pcl::RegionGrowingRGB::setNormalTestFlag
void setNormalTestFlag(bool value)
Allows to turn on/off the smoothness test.
Definition: region_growing_rgb.hpp:143
pcl::RegionGrowingRGB::setNumberOfRegionNeighbours
void setNumberOfRegionNeighbours(unsigned int nghbr_number)
This method allows to set the number of neighbours that is used for finding neighbouring segments.
Definition: region_growing_rgb.hpp:129
pcl::RegionGrowingRGB::calculateColorimetricalDifference
float calculateColorimetricalDifference(std::vector< unsigned int > &first_color, std::vector< unsigned int > &second_color) const
This method calculates the colorimetrical difference between two points.
Definition: region_growing_rgb.hpp:511
pcl::RegionGrowingRGB::getDistanceThreshold
float getDistanceThreshold() const
Returns the distance threshold.
Definition: region_growing_rgb.hpp:108
pcl::RegionGrowingRGB::~RegionGrowingRGB
~RegionGrowingRGB()
Destructor that frees memory.
Definition: region_growing_rgb.hpp:70
pcl::RegionGrowingRGB::getNormalTestFlag
bool getNormalTestFlag() const
Returns the flag that signalize if the smoothness test is turned on/off.
Definition: region_growing_rgb.hpp:136
pcl::RegionGrowingRGB::findRegionsKNN
void findRegionsKNN(pcl::index_t index, pcl::uindex_t nghbr_number, Indices &nghbrs, std::vector< float > &dist)
This method finds K nearest neighbours of the given segment.
Definition: region_growing_rgb.hpp:314
pcl::RegionGrowingRGB::RegionGrowingRGB
RegionGrowingRGB()
Constructor that sets default values for member variables.
Definition: region_growing_rgb.hpp:52
pcl::RegionGrowingRGB::setResidualTestFlag
void setResidualTestFlag(bool value) override
Allows to turn on/off the residual test.
Definition: region_growing_rgb.hpp:157
pcl::RegionGrowingRGB::findPointNeighbours
void findPointNeighbours() override
This method finds KNN for each point and saves them to the array because the algorithm needs to find ...
Definition: region_growing_rgb.hpp:273
pcl::RegionGrowingRGB::getRegionColorThreshold
float getRegionColorThreshold() const
Returns the color threshold value used for testing if regions can be merged.
Definition: region_growing_rgb.hpp:94
pcl::RegionGrowingRGB::findSegmentNeighbours
void findSegmentNeighbours()
This method simply calls the findRegionsKNN for each segment and saves the results for later use.
Definition: region_growing_rgb.hpp:295
pcl::RegionGrowingRGB::setPointColorThreshold
void setPointColorThreshold(float thresh)
This method specifies the threshold value for color test between the points.
Definition: region_growing_rgb.hpp:87
pcl::RegionGrowingRGB::validatePoint
bool validatePoint(index_t initial_seed, index_t point, index_t nghbr, bool &is_a_seed) const override
This function is checking if the point with index 'nghbr' belongs to the segment.
Definition: region_growing_rgb.hpp:599
pcl::search::KdTree
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
pcl::uindex_t
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition: types.h:120
pcl::comparePair
bool comparePair(std::pair< float, int > i, std::pair< float, int > j)
This function is used as a comparator for sorting.
Definition: region_growing.h:340
pcl::index_t
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition: types.h:112
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
pcl::PointIndices
Definition: PointIndices.h:12
pcl::PointIndices::indices
Indices indices
Definition: PointIndices.h:21