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