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