void start(const Mode mode) { this->mode = mode; running = true; std::string topicCameraInfoColor = topicColor.substr(0, topicColor.rfind('/')) + "/camera_info"; std::string topicCameraInfoDepth = topicDepth.substr(0, topicDepth.rfind('/')) + "/camera_info"; // std::cout << "test1..." << std::endl; image_transport::TransportHints hints("raw"); subImageColor = new image_transport::SubscriberFilter(it, topicColor, queueSize, hints); subImageDepth = new image_transport::SubscriberFilter(it, topicDepth, queueSize, hints); subCameraInfoColor = new message_filters::Subscriber<sensor_msgs::CameraInfo>(nh, topicCameraInfoColor, queueSize); subCameraInfoDepth = new message_filters::Subscriber<sensor_msgs::CameraInfo>(nh, topicCameraInfoDepth, queueSize); // std::cout << "test2..." << std::endl; syncExact = new message_filters::Synchronizer<ExactSyncPolicy>(ExactSyncPolicy(queueSize), *subImageColor, *subImageDepth, *subCameraInfoColor, *subCameraInfoDepth); syncExact->registerCallback(boost::bind(&Receiver::callback, this, _1, _2, _3, _4)); spinner.start(); std::cout << "waiting for kinect2_bridge..." << std::endl; std::chrono::milliseconds duration(1); while(!updateImage || !updateCloud) { if(!ros::ok()) { return; } std::this_thread::sleep_for(duration); } std::cout << "kinect2_bridge is running.." << std::endl; cloud = pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBA>()); cloud->height = color.rows; cloud->width = color.cols; cloud->is_dense = false; cloud->points.resize(cloud->height * cloud->width); createLookup(this->color.cols, this->color.rows); // // // Create the filtering object // // pcl::PassThrough<pcl::PointXYZRGBA> pass; // // pass.setInputCloud (cloud); // // pass.setFilterFieldName ("z"); // // pass.setFilterLimits (0.0, 1.0); // // //pass.setFilterLimitsNegative (true); // // pass.filter (*cloud); cloudViewer(); }
int main (int argc, char **argv) { std::cout << "main function" << std::endl; pcl::Grabber* interface; ros::NodeHandle *nh; ros::Subscriber sub; ros::Publisher clusterPub, normalPub, planePub,msgPub; bool spawnObject = true; K2G *k2g; processor freenectprocessor = OPENGL; boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB>> cloud; std::cout << "ros node initialized" << std::endl; ros::init(argc, argv, "hlpr_single_plane_segmentation",ros::init_options::NoSigintHandler); nh = new ros::NodeHandle("~"); // nh->getParam("segmentation/viz", viz_); // std::cout<<"viz is set to " << viz_ << endl; parsedArguments pA; if(parseArguments(argc, argv, pA, *nh) < 0) return 0; viz_ = pA.viz; ridiculous_global_variables::ignore_low_sat = pA.saturation_hack; ridiculous_global_variables::saturation_threshold = pA.saturation_hack_value; ridiculous_global_variables::saturation_mapped_value = pA.saturation_mapped_value; RansacSinglePlaneSegmentation multi_plane_app; pcl::PointCloud<PointT>::Ptr cloud_ptr (new pcl::PointCloud<PointT>); pcl::PointCloud<pcl::Normal>::Ptr ncloud_ptr (new pcl::PointCloud<pcl::Normal>); pcl::PointCloud<pcl::Label>::Ptr label_ptr (new pcl::PointCloud<pcl::Label>); boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer; multi_plane_app.initSegmentation(pA.seg_color_ind, pA.ecc_dist_thresh, pA.ecc_color_thresh); if(viz_) { viewer = cloudViewer(cloud_ptr); multi_plane_app.setViewer(viewer); } float workSpace[] = {-0.55,0.5,-0.2,0.45,0.3,2.0}; multi_plane_app.setWorkingVolumeThresholds(workSpace); msgPub = nh->advertise<hlpr_perception_msgs::SegClusters>(segOutRostopic,5); switch (pA.pc_source) { case 0: { std::cout << "Using ros topic as input" << std::endl; sub = nh->subscribe<sensor_msgs::PointCloud2>(pA.ros_topic, 1, cloud_cb_ros_); break; } case 1: { std::cout << "Using the openni device" << std::endl; interface = new pcl::OpenNIGrabber (); boost::function<void(const pcl::PointCloud<PointT>::ConstPtr&)> f = boost::bind (&cloud_cb_direct_,_1); boost::signals2::connection c = interface->registerCallback (f); interface->start (); break; } case 2: default: { std::cout << "Using kinect v2" << std::endl; freenectprocessor = static_cast<processor>(pA.freenectProcessor); k2g = new K2G(freenectprocessor, true); cloud = k2g->getCloud(); prev_cloud = cloud; gotFirst = true; break; } } signal(SIGINT, interruptFn); while (!interrupt && (!viz_ || !viewer->wasStopped ())) { if(pA.ros_node) { ros::spinOnce(); } if(viz_) viewer->spinOnce(20); if(!gotFirst) continue; int selected_cluster_index = -1; if(cloud_mutex.try_lock ()) { if(pA.pc_source == 2) { cloud = k2g->getCloud(); fake_cloud_cb_kinectv2_(cloud); } if(viz_ && pA.justViewPointCloud) { pcl::PointCloud<PointT>::Ptr filtered_prev_cloud(new pcl::PointCloud<PointT>(*prev_cloud)); multi_plane_app.preProcPointCloud(filtered_prev_cloud); if (!viewer->updatePointCloud<PointT> (filtered_prev_cloud, "cloud")) { viewer->addPointCloud<PointT> (filtered_prev_cloud, "cloud"); } selected_cluster_index = -1; } else { pcl::PointCloud<PointT>::CloudVectorType clusters; std::vector<pcl::PointCloud<pcl::Normal> > clusterNormals; Eigen::Vector4f plane; std::vector<size_t> clusterIndices; std::vector<std::vector<int>> clusterIndicesStore; selected_cluster_index = multi_plane_app.processOnce(prev_cloud,clusters,clusterNormals,plane,clusterIndicesStore, pA.pre_proc, pA.merge_clusters, viz_, pA.filterNoise); //true is for the viewer hlpr_perception_msgs::SegClusters msg; //msg.header.stamp = ros::Time::now(); // Pull out the cluster indices and put in msg for (int ti = 0; ti < clusterIndicesStore.size(); ti++){ hlpr_perception_msgs::ClusterIndex cluster_idx_msg; for (int j = 0; j < clusterIndicesStore[ti].size(); j++){ std_msgs::Int32 temp_msg; temp_msg.data = clusterIndicesStore[ti][j]; cluster_idx_msg.indices.push_back(temp_msg); } msg.cluster_ids.push_back(cluster_idx_msg); //clusterIndices.insert(clusterIndices.end(),clusterIndicesStore[ti].begin(), clusterIndicesStore[ti].end()); // For removing ALL cluster points } for(int i = 0; i < clusters.size(); i++) { sensor_msgs::PointCloud2 out; pcl::PCLPointCloud2 tmp; pcl::toPCLPointCloud2(clusters[i], tmp); pcl_conversions::fromPCL(tmp, out); msg.clusters.push_back(out); } for(int i = 0; i < clusterNormals.size(); i++) { sensor_msgs::PointCloud2 out; pcl::PCLPointCloud2 tmp; pcl::toPCLPointCloud2(clusterNormals[i], tmp); pcl_conversions::fromPCL(tmp, out); msg.normals.push_back(out); } std_msgs::Float32MultiArray planeMsg; planeMsg.data.clear(); for(int i = 0; i < 4; i++) planeMsg.data.push_back(plane[i]); //planePub.publish(planeMsg); msg.plane = planeMsg; msgPub.publish(msg); //clusterPub.publish(clusters[0]); //normalPub.publish(clusterNormals[0]); } cloud_mutex.unlock(); /*the z_thresh may result in wrong cluster to be selected. it might be a good idea to do * some sort of mean shift tracking, or selecting the biggest amongst the candidates (vs choosing the most similar color) * or sending all the plausible ones to c6 and decide there */ } if(selected_cluster_index < 0) continue; } if(pA.pc_source == 1) { interface->stop (); delete interface; } delete nh; ros::shutdown(); return 0; }
void run () { pcl::Grabber* interface = new pcl::OpenNIGrabber (); boost::function<void(const pcl::PointCloud<PointT>::ConstPtr&)> f = boost::bind (&OpenNIOrganizedMultiPlaneSegmentation::cloud_cb_, this, _1); //make a viewer pcl::PointCloud<PointT>::Ptr init_cloud_ptr (new pcl::PointCloud<PointT>); viewer = cloudViewer (init_cloud_ptr); boost::signals2::connection c = interface->registerCallback (f); interface->start (); unsigned char red [6] = {255, 0, 0, 255, 255, 0}; unsigned char grn [6] = { 0, 255, 0, 255, 0, 255}; unsigned char blu [6] = { 0, 0, 255, 0, 255, 255}; pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne; ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX); ne.setMaxDepthChangeFactor (0.03f); ne.setNormalSmoothingSize (20.0f); pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps; mps.setMinInliers (10000); mps.setAngularThreshold (0.017453 * 2.0); //3 degrees mps.setDistanceThreshold (0.02); //2cm std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions; pcl::PointCloud<PointT>::Ptr contour (new pcl::PointCloud<PointT>); size_t prev_models_size = 0; char name[1024]; while (!viewer->wasStopped ()) { viewer->spinOnce (100); if (prev_cloud && cloud_mutex.try_lock ()) { regions.clear (); pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>); double normal_start = pcl::getTime (); ne.setInputCloud (prev_cloud); ne.compute (*normal_cloud); double normal_end = pcl::getTime (); std::cout << "Normal Estimation took " << double (normal_end - normal_start) << std::endl; double plane_extract_start = pcl::getTime (); mps.setInputNormals (normal_cloud); mps.setInputCloud (prev_cloud); mps.segmentAndRefine (regions); double plane_extract_end = pcl::getTime (); std::cout << "Plane extraction took " << double (plane_extract_end - plane_extract_start) << std::endl; std::cout << "Frame took " << double (plane_extract_end - normal_start) << std::endl; pcl::PointCloud<PointT>::Ptr cluster (new pcl::PointCloud<PointT>); if (!viewer->updatePointCloud<PointT> (prev_cloud, "cloud")) viewer->addPointCloud<PointT> (prev_cloud, "cloud"); removePreviousDataFromScreen (prev_models_size); //Draw Visualization for (size_t i = 0; i < regions.size (); i++) { Eigen::Vector3f centroid = regions[i].getCentroid (); Eigen::Vector4f model = regions[i].getCoefficients (); pcl::PointXYZ pt1 = pcl::PointXYZ (centroid[0], centroid[1], centroid[2]); pcl::PointXYZ pt2 = pcl::PointXYZ (centroid[0] + (0.5f * model[0]), centroid[1] + (0.5f * model[1]), centroid[2] + (0.5f * model[2])); sprintf (name, "normal_%lu", i); viewer->addArrow (pt2, pt1, 1.0, 0, 0, false, name); contour->points = regions[i].getContour (); sprintf (name, "plane_%02zu", i); pcl::visualization::PointCloudColorHandlerCustom <PointT> color (contour, red[i], grn[i], blu[i]); viewer->addPointCloud (contour, color, name); viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, name); } prev_models_size = regions.size (); cloud_mutex.unlock (); } } interface->stop (); }