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