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; }
void cloud_cb_white (const sensor_msgs::PointCloud2ConstPtr& cloud_msg) { // Container for original & filtered data pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; pcl::PCLPointCloud2ConstPtr cloudPtr(cloud); pcl::PCLPointCloud2 cloud_filtered; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_segment (new pcl::PointCloud<pcl::PointXYZ>); // Convert to PCL data type pcl_conversions::toPCL(*cloud_msg, *cloud); // Perform downsampling pcl::VoxelGrid<pcl::PCLPointCloud2> sor; sor.setInputCloud (cloudPtr); sor.setLeafSize (0.01, 0.01, 0.01); sor.filter (cloud_filtered); // convert PointCloud2 to pcl::PointCloud<pcl::PointXYZ> pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::fromPCLPointCloud2(cloud_filtered,*temp_cloud); // segmenting area pcl::PassThrough<pcl::PointXYZ> pass; pass.setInputCloud (temp_cloud); // left - right pass.setFilterFieldName ("x"); pass.setFilterLimits (-3, 3); //pass.setFilterLimitsNegative (true); pass.filter (*cloud_segment); pass.setInputCloud (cloud_segment); // up - down pass.setFilterFieldName ("y"); pass.setFilterLimits (-3, 3); //pass.setFilterLimitsNegative (true); pass.filter (*cloud_segment); pass.setInputCloud (cloud_segment); // backward - forward pass.setFilterFieldName ("z"); pass.setFilterLimits (0.5, 4); //pass.setFilterLimitsNegative (true); pass.filter (*cloud_segment); // // Find centroid float sum_x = 0; float sum_y = 0; float sum_z = 0; if(cloud_segment->points.size() > 50) { for(size_t i = 0; i < cloud_segment->points.size(); ++i) { sum_x += cloud_segment->points[i].x; sum_y += cloud_segment->points[i].y; sum_z += cloud_segment->points[i].z; } sum_x = sum_x / cloud_segment->points.size(); sum_y = sum_y / cloud_segment->points.size(); sum_z = sum_z / cloud_segment->points.size(); geometry_msgs::PoseStamped sPose; sPose.header.stamp = ros::Time::now(); sPose.pose.position.x = sum_z; // equals to z in pcl // backward - forward sPose.pose.position.y = -(sum_x); // equals to -x in pcl // right - left sPose.pose.position.z = -(sum_y); // equals to -y in pcl // down - up sPose.pose.orientation.x = 0.0; sPose.pose.orientation.y = 0.0; sPose.pose.orientation.z = 0.0; sPose.pose.orientation.w = 1.0; cock_pose_white_pub.publish(sPose); } // Convert to ROS data type sensor_msgs::PointCloud2 output; //pcl_conversions::fromPCL(cloud_filtered, output); pcl::toROSMsg(*cloud_segment, output); // Publish the data cloud_pub_white.publish (output); }