746 lines
27 KiB
C++
746 lines
27 KiB
C++
/*
|
|
* 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_
|