void pointsCb(const PointCloud::ConstPtr& Callback_Points)
{
PointCloud::Ptr Points (new PointCloud);
*Points = *Callback_Points;
if(image == NULL)
return;
//添加rgb信息
cout<<"Points->width = "<<Points->width<<" Points->height = "<<Points->height<<"\n";
for (int j = 0; j < Points->width; j++)
{
for (int i = 0; i < Points->height; i++)
{
//float x = (float)(*input_cloud)(j,i).x;
//float y = (float)(*input_cloud)(j,i).y;
//float z = (float)(*input_cloud)(j,i).z;
int b = (int)CV_IMAGE_ELEM(image, uchar, i, j*3+0);
int g = (int)CV_IMAGE_ELEM(image, uchar, i, j*3+1);
int r = (int)CV_IMAGE_ELEM(image, uchar, i, j*3+2);
(*Points)(j,i).r = r;
(*Points)(j,i).g = g;
(*Points)(j,i).b = b;
}
}
viewer.showCloud(Points);
}
/***********************************************************************************************************************
* @BRIEF Callback function for received cloud data
* @PARAM[in] cloudIn the raw cloud data received by the OpenNI2 device
* @AUTHOR Christopher D. McMurrough
**********************************************************************************************************************/
void cloudCallback(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloudIn)
{
// get the elapsed time since the last callback
double elapsedTime = m_stopWatch.getTimeSeconds();
m_stopWatch.reset();
std::printf("Seconds elapsed since last cloud callback: %f \n", elapsedTime);
// store the cloud save count
static int saveCount = 0;
// render cloud if necessary
if(m_cloudRenderSetting)
{
m_viewer.showCloud(cloudIn);
}
// save the cloud if necessary
if(m_cloudSaveSetting)
{
std::stringstream ss;
string str;
ss << saveCount << ".pcd";
str = ss.str();
pcl::io::savePCDFile<pcl::PointXYZRGBA> (str.c_str(), *cloudIn, true);
std::printf("cloud saved to %s\n", str.c_str());
saveCount++;
}
}
void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
{
if (!viewer.wasStopped())
{
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr result (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PassThrough<pcl::PointXYZRGBA> pass;
pass.setFilterFieldName ("z");
pass.setFilterLimits (0.0, 3.0);
pass.setInputCloud (cloud);
pass.filter (*result);
s_cloud = *result;
viewer.showCloud (result);
}
}
void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
{
if (!viewer.wasStopped())
{
//// Green region detection
// pcl::PointCloud<pcl::PointXYZRGBA>::Ptr final_cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
// final_cloud->width = cloud->width;
// final_cloud->height = cloud->height;
// final_cloud->resize (cloud->width * cloud->height);
//
// size_t i = 0;
viewer.showCloud (cloud);
}
}
void run(){
depth=Mat(480,640,DataType<float>::type);
pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr nuage3(&nuage2);// (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointXYZRGBA point;
it=1000;
pcl::OpenNIGrabber* interface =new pcl::OpenNIGrabber();//creation d'un objet interface qui vient de l'include openni_grabber
//namedWindow( "Display Image", CV_WINDOW_AUTOSIZE );
namedWindow( "Harris Image", CV_WINDOW_AUTOSIZE );
//namedWindow( "Depth Image", CV_WINDOW_AUTOSIZE );
// VideoCapture capture(1);
// Mat frame;
// capture >> frame;
// record=VideoWriter("/home/guerric/Bureau/test.avi", CV_FOURCC('M','J','P','G'), 30, frame.size(), true);
boost::function<void(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)>
f = boost::bind (&ImageVIewer::cloud_cb_, this, _1);
boost::function<void(const boost::shared_ptr<openni_wrapper::Image>&)>
g = boost::bind (&ImageVIewer::image_cb_, this, _1);
boost::function<void(const boost::shared_ptr<openni_wrapper::DepthImage>&)>
h = boost::bind (&ImageVIewer::depth_cb_, this, _1);
interface->registerCallback (f);
interface->registerCallback (g);
interface->registerCallback (h);
interface->start();
//on reste dans cet état d'acquisition tant qu'on ne stoppe pas dans le viewer
while(!viewer.wasStopped()){
boost::this_thread::sleep(boost::posix_time::seconds(1)); //met la fonction en attente pendant une seconde <=> sleep(1) mais plus précis pour les multicores
viewer.showCloud(nuage3);
}
interface->stop();
record.release();
destroyAllWindows();
}
//描画
void display() {
// clear screen and depth buffer
glClear ( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
// Reset the coordinate system before modifying
glLoadIdentity();
glEnable(GL_DEPTH_TEST); //「Zバッファ」を有効
gluLookAt(0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 0.0); //視点の向き設定
if(GetImage(image,m_hNextImageFrameEvent,m_pImageStreamHandle)==-1)
return;
if(GetImage(depth,m_hNextDepthFrameEvent,m_pDepthStreamHandle)==-1)
return;
//3次元ポイントクラウドのための座標変換
//retrievePointCloudMap(depth,pointCloud_XYZ);
retrieveRGBCloudMap(depth,pointCloud_XYZforRGB);
//視点の変更
polarview();
imshow("image",image);
imshow("depth",depth);
//RGBAからBGRAに変換
cvtColor(image,image,CV_RGBA2BGRA);
//ポイントクラウド
//drawPointCloud(image,pointCloud_XYZ,depth);
drawPointCloud_easy(image,pointCloud_XYZforRGB);
glFlush();
glutSwapBuffers();
//Draw on PCL viewer
Mat2PCL_XYZRGB_ALL(image,pointCloud_XYZforRGB,*cloud);
PCLviewer.showCloud(cloud);
}
void cloud_cb(const boost::shared_ptr<const sensor_msgs::PointCloud2>& rototranslatedpcBoostPtr){
boost::shared_ptr <pcl::PointCloud <pcl::PointXYZ> > pclCloudBoostPtr (new pcl::PointCloud<pcl::PointXYZ> );
pcl::fromROSMsg( *rototranslatedpcBoostPtr , *pclCloudBoostPtr ); // ORIG WORKING
// // Perform voxel filter
// boost::shared_ptr <pcl::PointCloud <pcl::PointXYZ> > filteredCloudBoostPtr (new pcl::PointCloud<pcl::PointXYZ> ); // Uncomment to use filtering
// pcl::VoxelGrid<pcl::PointXYZ> sor;
// sor.setInputCloud (pclCloudBoostPtr);
// sor.setLeafSize (0.01f, 0.01f, 0.01f);
// sor.filter (*filteredCloudBoostPtr);
//
// // Prints filtered pointcloud
// for (size_t i = 0; i < filteredCloudBoostPtr->points.size (); ++i)
// std::cout << filteredCloudBoostPtr->points[i].x
// << " "<< filteredCloudBoostPtr->points[i].y
// << " "<< filteredCloudBoostPtr->points[i].z << std::endl;
//
// std::cout<<filteredCloudBoostPtr->points.size ()<<std::endl;
// if (!viewer.wasStopped()) viewer.showCloud (filteredCloudBoostPtr, "sample cloud");
if (!viewer.wasStopped()) viewer.showCloud ( pclCloudBoostPtr, "sample cloud"); // IGNORE ECLIPSE ERROR HERE. COMPILER WORKS.
}
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
}
}
void show_cloud( pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
{
viewer.showCloud(cloud);
}
void cloud_cb_(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud){ //fonction <> =>classe template
if(!viewer.wasStopped()){
viewer.showCloud(cloud);//on montre le viewer tant qu'on ne l'a pas arreté
}
}
int main (int argc, char** argv)
{
//----------------------------------------------------------------------------------
//Read pcd file
//----------------------------------------------------------------------------------
if (pcl::io::loadPCDFile<PoinT> ("Cosyslab-0.pcd", *source_cloud) == -1) //* load the file
{
PCL_ERROR ("Couldn't read file source_cloud.pcd \n");
return (-1);
}
cout << "Loaded " << source_cloud->width * source_cloud->height << " data points "<< endl;
if (pcl::io::loadPCDFile<PoinT> ("Cosyslab-1.pcd", *target_cloud) == -1) //* load the file
{
PCL_ERROR ("Couldn't read file target_test_nonoise.pcd \n");
return (-1);
}
cout << "Loaded " << target_cloud->width * target_cloud->height << " data points "<< endl;
//----------------------------------------------------------------------------------
//remove NAN points from the cloud
//----------------------------------------------------------------------------------
std::vector<int> indices_src, indices_tgt;
pcl::removeNaNFromPointCloud(*source_cloud,*source_cloud, indices_src);
pcl::removeNaNFromPointCloud(*target_cloud,*target_cloud, indices_tgt);
//----------------------------------------------------------------------------------
//Reduce number of points
//----------------------------------------------------------------------------------
pcl::VoxelGrid<PoinT> grid, grid1;
grid.setLeafSize (0.01, 0.01, 0.05);
grid.setInputCloud (source_cloud);
grid.filter(*source_cloud);
cout << "source cloud number of point after voxelgrid: " << source_cloud->points.size() << endl;
grid1.setLeafSize (0.01, 0.01, 0.05);
grid1.setInputCloud (target_cloud);
grid1.filter(*target_cloud);
cout << "target cloud number of point after voxelgrid: " << target_cloud->points.size() << endl;
//----------------------------------------------------------------------------------
//Make source cloud blue
//----------------------------------------------------------------------------------
for (int i = 0; i < source_cloud->points.size(); ++i) {
source_cloud->points[i].r = 0;
source_cloud->points[i].g = 0;
source_cloud->points[i].b = 255;
}
//----------------------------------------------------------------------------------
//Make Target cloud Red
//----------------------------------------------------------------------------------
for (int i = 0; i < target_cloud->points.size(); ++i) {
target_cloud->points[i].r = 255;
target_cloud->points[i].g = 0;
target_cloud->points[i].b = 0;
}
//----------------------------------------------------------------------------------
//Apply PCA transformation from target to source
//----------------------------------------------------------------------------------
if(PCAregistration == true){
Eigen::Vector4f centroid_source, centroid_target;
Eigen::Matrix4f transformationm_source, transformationm_target, PCAtransformation;
applyPCAregistration(source_cloud, centroid_source, transformationm_source);
applyPCAregistration(target_cloud, centroid_target, transformationm_target);
PCAtransformation = transformationm_source * transformationm_target.transpose();
//Apply rotation transformation
pcl::transformPointCloud(*source_cloud, *transformed_cloud, PCAtransformation);
//calculate fitnesscore
FitnesscorePCA = calculateFitnesscore(target_cloud,source_cloud, PCAtransformation);
cout << "FitnesscorePCA is : " << FitnesscorePCA << " meter "<< endl;
}
//----------------------------------------------------------------------------------
//Apply SVD transformation from target to source
//----------------------------------------------------------------------------------
if(SVDregistration == true){
Eigen::Matrix4f trans_matrix_svd;
applySVDregistration(source_cloud, target_cloud, trans_matrix_svd);
pcl::transformPointCloud (*source_cloud, *transformed_cloud, trans_matrix_svd);
//calculate fitnesscore
FitnesscoreSVD = calculateFitnesscore(target_cloud,source_cloud, trans_matrix_svd);
cout << "FitnesscoreSVD is : " << FitnesscoreSVD << " meter "<< endl;
}
//----------------------------------------------------------------------------------
//Make transformed cloud White
//----------------------------------------------------------------------------------
for (int i = 0; i < transformed_cloud->points.size(); ++i) {
transformed_cloud->points[i].r = 255;
transformed_cloud->points[i].g = 255;
transformed_cloud->points[i].b = 255;
}
//----------------------------------------------------------------------------------
//Show cloud in viewer
//----------------------------------------------------------------------------------
viewer.showCloud (source_cloud, "source");
viewer.showCloud (transformed_cloud, "transformed");
viewer.showCloud (target_cloud, "target");
while (!viewer.wasStopped ())
{
//while keypress "q" is pressed
}
return (0);
}
void cloud_cb_ (const sensor_msgs::PointCloud2ConstPtr& input)
{
if (!viewer.wasStopped()){
// if(!cloud_received){
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr in_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
//pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::fromROSMsg (*input, *in_cloud);
//downsample
pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsampled_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
downSample(in_cloud, downsampled_cloud);
//crop
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cropped_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::IndicesPtr indices (new std::vector <int>);
cropCloud(downsampled_cloud, cropped_cloud, indices);
//rotate
pcl::PointCloud<pcl::PointXYZRGB>::Ptr rotated_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
rotateCloud(cropped_cloud, rotated_cloud);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
colorSegment(rotated_cloud, filtered_cloud);
/*
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (msg);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
PCL_ERROR ("Could not estimate a planar model for the given dataset.");
return;
//return (-1);
}
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;
cloud.points.resize(inliers->indices.size ());
cloud.width=1;
cloud.height= inliers->indices.size ();
for (size_t i = 0; i < inliers->indices.size (); ++i){
cloud.points[i].x = (*msg).points[inliers->indices[i]].x;
cloud.points[i].y = (*msg).points[inliers->indices[i]].y;
cloud.points[i].z = (*msg).points[inliers->indices[i]].z;
//std::cerr << inliers->indices[i] << " " << (*msg).points[inliers->indices[i]].x << " "
// << (*msg).points[inliers->indices[i]].y << " "
// << (*msg).points[inliers->indices[i]].z << std::endl;
}
//cloud_received = true;
}
*/
//return (0);
cv::waitKey(3);
viewer.showCloud(filtered_cloud);
//viewer.showCloud(msg);
}
}