std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> MovingObjectsIdentificator::identify() {
if(verbose) std::cout << "\nMoving Object Identification start" << std::endl;
findDifference();
removeOutliers();
if(verbose) std::cout << "Moving Object Identification end" << std::endl;
return extractClusters();
}
void MultiDLA::clusterAggregation(CellsField* fld, size_t clusterCnt)
{
size_t targetClusterCnt = clusterCnt;
uint32_t maxClusterMoves = 10;
MCoordVec* directions = createDirections();
uint32_t iter = 0;
uint32_t iterstep = 20;
uint32_t maxSize = 0;
{
CellsField* marked = markClusters(fld);
std::vector<MCoordVec>* clusters = extractClusters(marked);
maxSize = uint32_t(clusters->size());
if (maxSize < 1) {
maxSize = 1;
}
delete marked;
delete clusters;
}
while (true) {
if (m_cancel) {
break;
}
CellsField* marked = markClusters(fld);
std::vector<MCoordVec>* clusters = extractClusters(marked);
delete marked;
std::cout << "New iter. Clusters: " << clusters->size() << std::endl;
if (clusters->size() <= targetClusterCnt) {
QMetaObject::invokeMethod(m_mainwindow, "setProgress", Qt::QueuedConnection,
Q_ARG(int, std::min(100, int(100 * (maxSize - clusters->size() + targetClusterCnt)) / int(maxSize))));
delete clusters;
break;
}
++iter;
if (iter % iterstep == 0) {
iter = 0;
QMetaObject::invokeMethod(m_mainwindow, "setProgress", Qt::QueuedConnection,
Q_ARG(int, std::min(100, int(100 * (maxSize - clusters->size() + targetClusterCnt)) / int(maxSize))));
QMetaObject::invokeMethod(m_mainwindow, "restructGL", Qt::QueuedConnection);
}
int EuclideanClustering::segment(){
assert (this->inputPointCloud != 0);
extractClusters(this->inputPointCloud);
return 1;
}
void segmentObstaclesFromGround(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const typename pcl::IndicesPtr & indices,
pcl::IndicesPtr & ground,
pcl::IndicesPtr & obstacles,
float normalRadiusSearch,
float groundNormalAngle,
int minClusterSize,
bool segmentFlatObstacles)
{
ground.reset(new std::vector<int>);
obstacles.reset(new std::vector<int>);
if(cloud->size())
{
// Find the ground
pcl::IndicesPtr flatSurfaces = normalFiltering(
cloud,
indices,
groundNormalAngle,
Eigen::Vector4f(0,0,1,0),
normalRadiusSearch*2.0f,
Eigen::Vector4f(0,0,100,0));
if(segmentFlatObstacles)
{
int biggestFlatSurfaceIndex;
std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = extractClusters(
cloud,
flatSurfaces,
normalRadiusSearch*2.0f,
minClusterSize,
std::numeric_limits<int>::max(),
&biggestFlatSurfaceIndex);
// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
Eigen::Vector4f min,max;
pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
{
if((int)i!=biggestFlatSurfaceIndex)
{
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
if(centroid[2] >= min[2] && centroid[2] <= max[2])
{
ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
}
}
}
}
else
{
ground = flatSurfaces;
}
if(ground->size() != cloud->size())
{
// Remove ground
pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, ground, true);
//Cluster remaining stuff (obstacles)
std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
cloud,
otherStuffIndices,
normalRadiusSearch*2.0f,
minClusterSize);
// merge indices
obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
}
}
}