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();

}
Пример #2
0
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);
        }
Пример #3
0
int EuclideanClustering::segment(){

	assert (this->inputPointCloud != 0);
	extractClusters(this->inputPointCloud);
	return 1;
}
Пример #4
0
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);
		}
	}
}