void Segmentation::ransac(std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > &clusters, Eigen::Vector3f axis, std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > &vectorCloudInliers )
{
vectorCloudInliers.clear();
for(unsigned int i = 0; i < clusters.size(); i++)
{
pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr indices(new pcl::PointIndices);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_(new pcl::PointCloud<pcl::PointXYZRGBA>);
//pcl::PointCloud<pcl::PointXYZRGBA>::Ptr hullGround(new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::NormalEstimation<pcl::PointXYZRGBA, pcl::Normal> ne;
pcl::SACSegmentationFromNormals<pcl::PointXYZRGBA, pcl::Normal> seg;
pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBA> ());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
// Estimate Normals
ne.setSearchMethod(tree);
ne.setInputCloud(clusters[i]);
ne.setKSearch(50);
ne.compute(*cloud_normals);
// Create the segmentation object
//pcl::SACSegmentation<pcl::PointXYZRGBA> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
//seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE); // We search a plane perpendicular to the y
seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);// We search a plane perpendicular to the y
seg.setNormalDistanceWeight(0.005);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.020); //0.25 / 35
//seg.setAxis(axis); // Axis Y 0, -1, 0
seg.setEpsAngle(20.0f * (M_PI/180.0f)); // 50 degrees of error
pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
seg.setInputCloud (clusters[i]);
seg.setInputNormals(cloud_normals);
seg.segment (*indices, *coeff);
if(normalDifferenceError(coeff))
{
std::cout << "coeff" << coeff->values[0] << " " << coeff->values[1] << " " << coeff->values[2] << std::endl;
if (indices->indices.size () == 0)
{
PCL_ERROR ("Could not estimate a planar model (Ground).");
//return false;
}
else
{
extract.setInputCloud(clusters[i]);
extract.setIndices(indices);
extract.setNegative(false);
extract.filter(*cloud_);
vectorCloudInliers.push_back(cloud_);
//return true;
}
}
}
}
int main(int argc,char**argv)
{
signal(SIGINT,sigint_handler);
srand(time(NULL));
int r_ = rand()%255;int g_ = rand()%255;int b_ = rand()%255;
int i=0;
kinect::Kinect2Grabber obj;
kinect::FrameData data;
//std::vector<int> compress;
//compress.push_back(CV_IMWRITE_JPEG_QUALITY );
//compress.push_back(100);
//compress.push_back(CV_IMWRITE_PNG_COMPRESSION);
//compress.push_back(2);
cv::namedWindow( "registered",CV_WINDOW_AUTOSIZE );
//cv::namedWindow( "Depth",CV_WINDOW_AUTOSIZE );
//cv::namedWindow( "Ir",CV_WINDOW_AUTOSIZE );
data = obj.getRegisteredImage();
//data = obj.getFrameData();
pcl::PointCloud<PointT>::Ptr cloud_(new pcl::PointCloud<PointT>());
pcl::PointCloud<PointT>::Ptr cloud_segment(new pcl::PointCloud<PointT>());
cloud_->width = data.depth_.cols;
cloud_->height = data.depth_.rows;
cloud_->points.resize(data.depth_.rows*data.depth_.cols);
pcl::visualization::PCLVisualizer* viewer(new pcl::visualization::PCLVisualizer ("Kinect Cloud"));
//pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud_);
viewer->addPointCloud<PointT>(cloud_,"Kinect Cloud");
//pcl::visualization::PointCloudColorHandlerCustom <PointT> color_handle(cloud_segment,r_,g_,b_);
//viewer->addPointCloud<PointT>(cloud_segment,"Kinect Cloud");
viewer->setBackgroundColor (0, 0, 0);
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3, "Kinect Cloud");
//viewer->addCoordinateSystem (1.0);
viewer->initCameraParameters ();
/*
//segment
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<PointT> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (cloud_);
*/
while(key!=99)
{
data = obj.getRegisteredImage();
//data = obj.getFrameData();
cv::imshow("registered",data.color_);
//cv::imshow("Depth",data.depth_);
//cv::imshow("Ir",data.IR_);
key = cv::waitKey(10);
libfreenect2::Freenect2Device::IrCameraParams params = obj.getIrParams();
std::cout<<"Camera centre : "<<params.cx<<" "<<params.cy<<std::endl;
std::cout<<"Focal parameters : "<<params.fx<<" "<<params.fy<<std::endl;
int size_r = data.color_.rows;
int size_c = data.color_.cols;
int index = 0;
#pragma omp parallel for
for(int i=0;i<size_r;i++)
{
for(int j=0;j<size_c;j++,index++)
{
float depth_pos = data.depth_.at<float>(i,j);
float pos_x = (float)((j-params.cx)*depth_pos)/params.fx;
float pos_y = (float)((i-params.cy)*depth_pos)/params.fy;
cloud_->points[index].x = pos_x;
cloud_->points[index].y = pos_y;
cloud_->points[index].z = depth_pos;
const cv::Vec3b tmp = data.color_.at<cv::Vec3b>(i,j);
cloud_->points[index].b = tmp.val[0];
cloud_->points[index].g = tmp.val[1];
cloud_->points[index].r = tmp.val[2];
}
}
/*
if(segment){
seg.segment (*inliers, *coefficients);
for(unsigned int i=0;i<inliers->indices.size();i++)
{
cloud_->points[inliers->indices[i]].r = 255;
cloud_->points[inliers->indices[i]].g = 0;
cloud_->points[inliers->indices[i]].b = 0;
}
}
*/
///////////////////////////////////////////////
//Multi Plane Segmentation
///////////////////////////////////////////////
pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
ne.setInputCloud (cloud_);
ne.compute (*normal_cloud);
float* distance_map = ne.getDistanceMap ();
boost::shared_ptr<pcl::EdgeAwarePlaneComparator<PointT,pcl::Normal> > eapc(new pcl::EdgeAwarePlaneComparator<PointT, pcl::Normal> ());
eapc->setDistanceMap (distance_map);
eapc->setDistanceThreshold (0.01f, false);
std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
std::vector<pcl::ModelCoefficients> model_coefficients;
std::vector<pcl::PointIndices> inlier_indices;
pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>);
std::vector<pcl::PointIndices> label_indices;
std::vector<pcl::PointIndices> boundary_indices;
mps.setInputNormals (normal_cloud);
mps.setInputCloud (cloud_);
mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
//////////////////view contours of segmented planes////////////////////////////
if(contours_only)
{
std::vector<PointT,Eigen::aligned_allocator<PointT> > c_points;
int index_s=0;
unsigned int size_region = regions.size();
long int cloud_size;
for(unsigned int i=0;i<size_region;i++)
{
cloud_size+=regions[i].getContour().size();
}
cloud_segment->points.resize(cloud_size);
for(unsigned int i=0;i<regions.size();i++)
{
c_points = regions[i].getContour();
r_ = rand()%255;g_ = rand()%255;b_ = rand()%255;
for(unsigned int j=0;j<c_points.size();j++,index_s++)
{
cloud_segment->points[index_s].x = c_points[j].x;
cloud_segment->points[index_s].y = c_points[j].y;
cloud_segment->points[index_s].z = c_points[j].z;
cloud_segment->points[index_s].r = r_;
cloud_segment->points[index_s].g = g_;
cloud_segment->points[index_s].b = b_;
}
}
}
//////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//Using boundary indices
///////////////////////////////////////////////////////////////////////////////
else{
/*
for(unsigned int i=0;i<boundary_indices.size();i++)
{
r_ = rand()%255;g_ = rand()%255;b_ = rand()%255;
for(unsigned int j=0;j<boundary_indices[i].indices.size();j++)
{
cloud_->points[boundary_indices[i].indices[j]].r = r_;
cloud_->points[boundary_indices[i].indices[j]].g = g_;
cloud_->points[boundary_indices[i].indices[j]].b = b_;
}
}
*/
for(unsigned int i=0;i<label_indices.size();i++)
{
r_ = rand()%255;g_ = rand()%255;b_ = rand()%255;
for(unsigned int j=0;j<label_indices[i].indices.size();j++)
{
cloud_->points[label_indices[i].indices[j]].r = r_;
cloud_->points[label_indices[i].indices[j]].g = g_;
cloud_->points[label_indices[i].indices[j]].b = b_;
}
}
}
//viewer->updatePointCloud(cloud_segment,"Kinect Cloud");
viewer->updatePointCloud(cloud_,"Kinect Cloud");
viewer->spinOnce (100);
if(print&&(key==99)){
cloud_->height = 1;
cloud_->width = cloud_->points.size();
pcl::io::savePCDFileASCII ("test_new.pcd",*cloud_);
print = false;
}
}
return 0;
}
