Ejemplo n.º 1
0
void FindFloorPlaneRANSAC()
{	
	double t = getTickCount();
	cout << "RANSAC...";
	/*
	 pcl::SampleConsensusModelPlane<pcl::PointXYZRGB>::Ptr model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZRGB> (cloud));
	 */
	pcl::SampleConsensusModelNormalPlane<pcl::PointXYZRGB,pcl::Normal>::Ptr model_p(
					new pcl::SampleConsensusModelNormalPlane<pcl::PointXYZRGB,pcl::Normal>(cloud));
	//	model_p->setInputCloud(cloud);
	model_p->setInputNormals(mls_normals);
	model_p->setNormalDistanceWeight(0.75);
	
	inliers.reset(new vector<int>);
	
	ransac.reset(new pcl::RandomSampleConsensus<pcl::PointXYZRGB>(model_p));
	ransac->setDistanceThreshold (.1);
	ransac->computeModel();	
	ransac->getInliers(*inliers);
	
	t = ((double)getTickCount() - t)/getTickFrequency();
	cout << "Done. (" << t <<"s)"<< endl;
	
	ransac->getModelCoefficients(coeffs[0]);
	model_p->optimizeModelCoefficients(*inliers,coeffs[0],coeffs[1]);
	
	floorcloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::copyPointCloud(*cloud,*inliers,*floorcloud);

}
Ejemplo n.º 2
0
void segmentObstaclesFromGround(
		const typename pcl::PointCloud<PointT>::Ptr & cloud,
		pcl::IndicesPtr & ground,
		pcl::IndicesPtr & obstacles,
		float normalRadiusSearch,
		float groundNormalAngle,
		int minClusterSize)
{
	ground.reset(new std::vector<int>);
	obstacles.reset(new std::vector<int>);

	// Find the ground
	pcl::IndicesPtr flatSurfaces = util3d::normalFiltering<PointT>(
			cloud,
			groundNormalAngle,
			Eigen::Vector4f(0,0,1,0),
			normalRadiusSearch*2.0f,
			Eigen::Vector4f(0,0,100,0));

	int biggestFlatSurfaceIndex;
	std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = util3d::extractClusters<PointT>(
			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<PointT>(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);

	for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
	{
		if((int)i!=biggestFlatSurfaceIndex)
		{
			Eigen::Vector4f centroid;
			pcl::compute3DCentroid<PointT>(*cloud, *clusteredFlatSurfaces.at(i), centroid);
			if(centroid[2] >= min[2] && centroid[2] <= max[2])
			{
				ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
			}
		}
	}

	if(ground->size() != cloud->size())
	{
		// Remove ground
		pcl::IndicesPtr otherStuffIndices = util3d::extractNegativeIndices<PointT>(cloud, ground);

		//Cluster remaining stuff (obstacles)
		std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters<PointT>(
				cloud,
				otherStuffIndices,
				normalRadiusSearch*2.0f,
				minClusterSize);

		// merge indices
		obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
	}
}