template <typename PointT, typename PointNT, typename PointLabelT, typename OptionsT, typename SensorT, typename ClusterHdlT> bool
cob_3d_segmentation::FastSegmentation<PointT,PointNT,PointLabelT,OptionsT,SensorT,ClusterHdlT>::compute()
{
clusters_->setPointCloudIn(surface_);
clusters_->setNormalCloudIn(normals_);
clusters_->setLabelCloudInOut(labels_);
graph_->edges()->setPointCloudIn(surface_);
graph_->edges()->setLabelCloudIn(labels_);
createSeedPoints();
int w = labels_->width, h = labels_->height, s = 1;
int mask[4][3] = { { -s, -w, s }, // from below
{ -w, s, w }, // from left
{ s, w, -s }, // from above
{ w, -s, -w } }; // from right
int mask_size = 3;
clusters_->clear();
clusters_->createCluster(I_NAN);
while(seeds_.size() != 0)
{
unsigned int idx = seeds_.front();
seeds_.pop_front();
ClusterPtr c = clusters_->createCluster();
std::list<SeedPoint::Ptr> seg_q;
seg_q.push_back( SeedPoint::Ptr(new SeedPoint(idx, 0, 0.0)) );
clusters_->addPoint(c, idx);
(*labels_)[idx].label = c->id();
while(seg_q.size() != 0)
{
SeedPoint p = *seg_q.front();
seg_q.pop_front();
for(int i=0; i<mask_size; ++i)
{
int i_idx = p.idx + mask[p.i_came_from][i];
if (i_idx >= w*h || i_idx < 0) continue;
if(!SensorT::areNeighbors((*surface_)[p.idx].getVector3fMap(), (*surface_)[i_idx].getVector3fMap(),4.0f)) continue;
int* p_label = &(labels_->points[ i_idx ]).label;
if( hasLabel(*p_label, c->id(), i_idx, p.idx, OptionsT()) ) continue;
Eigen::Vector3f i_n = c->getOrientation();
//Eigen::Vector3f i_c = c->getCentroid();
float dot_value = fabs( i_n.dot((*normals_)[i_idx].getNormalVector3fMap()) );
if(dot_value > n_threshold(c->size()))
{
seg_q.push_back( SeedPoint::Ptr(new SeedPoint(i_idx, LISTMOD(i + p.i_came_from - 1, 4), dot_value)) );
clusters_->addPoint(c,i_idx);
seeds_.erase(p_seeds_[i_idx]);
*p_label = c->id();
}
}
}
}
clusters_->addBorderIndicesToClusters();
return true;
}
QString Section::toString(bool debug, short level) const {
QString str;
if(hasLabel()) str += QString(" ").repeated(level - 1) + label()->toString(debug, level) + ":";
str = concatenateStrings(str, "\n", lines()->join("\n", QString(" ").repeated(level), "", debug, level));
return str;
}
