int main (int argc, char** argv) { // Initialize ROS ros::init (argc, argv, "objectMaster"); ros::NodeHandle n; ros::Rate waiting_rate(30); //strat a traver and wait for its ready cloudTraver ct(n); while(!ct.isReady()) { ros::spinOnce(); waiting_rate.sleep(); } cvNamedWindow("CurrentImage",CV_WINDOW_AUTOSIZE); cv::Mat image; pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudP; Mat src, dst, color_dst; std::string objectNameTemp="TEMP"; int count=0; while(ros::ok()) { while(!ct.isReady()) { ros::spinOnce(); } image=ct.getImage(); cloudP=ct.getCloud(); if(cloudP->empty()) { std::cout<<"No pointCloud passed into this process, f**k you no points you play MAOXIAN!"<<std::endl; continue; } pcl::PointCloud<PointType>::Ptr cloud_RGBA(new pcl::PointCloud<PointType>); *cloud_RGBA=*cloudP; Filter filter; cloud_RGBA=filter.PassThrough(cloud_RGBA); cloud_RGBA=filter.DeSamping(cloud_RGBA); cloud_RGBA=filter.RemovePlane(cloud_RGBA); if(cloud_RGBA->empty()) { std::cout<<"No pointCloud left after the samping"<<std::endl; continue; } std::vector<pcl::PointIndices> cluster_indices; filter.ExtractionObject(cloud_RGBA,cluster_indices); if(cluster_indices.size()!=0) { std::cout<<cluster_indices.size()<<"clusters extraced"<<std::endl; pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGBA>); pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGBA>); pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud = cloud_RGBA; //voxelgrid并不是产生球面空洞的原因 pcl::VoxelGrid<pcl::PointXYZRGBA> vg; vg.setInputCloud (cloud); vg.setLeafSize (0.01f, 0.01f, 0.01f); vg.filter (*cloud_filtered); //Create the segmentation object for the planar model and set all the parameters pcl::SACSegmentation<pcl::PointXYZRGBA> seg; pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZRGBA> ()); pcl::PCDWriter writer; seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.02); //......... int i=0, nr_points = (int) cloud_filtered->points.size (); while (cloud_filtered->points.size () > 0.3 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cout << "Could not estimate a planar model for the given dataset." << std::endl; break; } // Extract the planar inliers from the input cloud pcl::ExtractIndices<pcl::PointXYZRGBA> extract; extract.setInputCloud (cloud_filtered); extract.setIndices (inliers); extract.setNegative (false); // Get the points associated with the planar surface extract.filter (*cloud_plane); //std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl; // Remove the planar inliers, extract the rest extract.setNegative (true); extract.filter (*cloud_f); *cloud_filtered = *cloud_f; } //cloud filter ! pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBA>); if ( !cloud_filtered->empty())tree->setInputCloud (cloud_filtered); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZRGBA> ec; ec.setClusterTolerance (0.04); // 4cm ec.setMinClusterSize (200); ec.setMaxClusterSize (1000); ec.setSearchMethod (tree); ec.setInputCloud (cloud_filtered); ec.extract (cluster_indices); int j = 0; int cnt = 0,inlaw = 0; //start detect for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) { pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGBA>); for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit) cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //* cloud_cluster->width = cloud_cluster->points.size (); cloud_cluster->height = 1; cloud_cluster->is_dense = true; //std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl; //std::stringstream ss; //ss << "cloud_cluster_" << j << ".pcd"; //writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //* j++; cout <<"......................................................."<<endl; cout << "cloud_cluster_" << j << endl; if ( match(cloud_cluster)) { cout << ++cnt << " sphere have been detected " <<endl; unsigned int red,green,blue; for (int i=0,red = 0,green =0,blue = 0; i < cloud_cluster->points.size(); i++) { red =cloud_cluster->points[i].r; green =cloud_cluster->points[i].g; blue =cloud_cluster->points[i].b; if(getDistance(red,green,blue))inlaw++; } unsigned int tot = cloud_cluster->points.size(); red /=tot; blue /=tot; green /=tot; /// compared with standard red ! int dis = 0; dis = (red-255)*(red-255)+(blue-0)*(blue-0)+(green-0)*(green-0); if(inlaw > cloud_cluster->points.size()*0.7) { cout <<"This is red ball ! "<<endl; } else { cout <<"Sorry it's not red ball ...."<<endl; cout <<"#Average -> R :"<<red<<" G: "<<green<<" B: "<<blue<<endl; } cout <<"------------------------------------------------------"<<endl; //viewer.showCloud(cloud_cluster); } } } viewer.showCloud(cloud_RGBA); // --------OpenCV---------- Mat src, dst, color_dst; cvtColor(image, image, CV_BGR2GRAY); //sharpenImage1(image, image); GaussianBlur(image, image, Size(5, 5), 1.5); //sharpenImage1(image, image); vector<Vec3f> circles; HoughCircles(image, circles, CV_HOUGH_GRADIENT,2,120,220,70,25, 300); vector<Vec3f>::iterator itc = circles.begin(); while(itc != circles.end()) { cout << "x:" << (*itc)[0] << "y:" << (*itc)[1] << endl; circle(image,Point((*itc)[0], (*itc)[1]),(*itc)[2],Scalar(255),2); //颜色//厚度 x_position[count] = (*itc)[0]; //cout << "x_position" << count << ":" << x_position << endl; count++; if (count == 5) { sort(x_position, x_position + 5); int mid_value = x_position[2];//每5帧的中位数 cout << "mid_value:" << mid_value << endl; cout << "............................." << endl; double r = (*itc)[2]; //double d = 640 * 230/(2 * r) - 271; int l = (mid_value - 320); double theta = (double)52 / 640 * l; cout << "theta:" << theta << endl; if (mid_value > 410) { cout << "need to turn right" << endl; direction = 1; } else if (mid_value < 230) { cout << "need to turn left" << endl; direction = -1; } else { cout << "succeed" << endl; direction = 0; } count = 0; mid_value = 0; } ++itc; } // viewerS.showCloud(cloudP); cv::imshow("CurrentImage", image); //U must wait a second to let the image show on the screen char temp=cvWaitKey(0); if (temp ==27)break; //wait for new datas } }