void OrganizedSegmentationDemo::timeoutSlot () { { QMutexLocker vis_locker (&vis_mtx_); if (capture_ && data_modified_) { removePreviousDataFromScreen (previous_data_size_, previous_clusters_size_, vis_); if (!vis_->updatePointCloud (boost::make_shared<pcl::PointCloud<PointT> >(prev_cloud_), "cloud")) { vis_->addPointCloud (boost::make_shared<pcl::PointCloud<PointT> >(prev_cloud_), "cloud"); vis_->resetCameraViewpoint ("cloud"); } displayPlanarRegions (prev_regions_, vis_); if (display_curvature_) displayCurvature (prev_cloud_, prev_normals_, vis_); else vis_->removePointCloud ("curvature"); if (display_distance_map_) displayDistanceMap (prev_cloud_, prev_distance_map_, vis_); else vis_->removePointCloud ("distance_map"); if (display_normals_) { vis_->removePointCloud ("normals"); vis_->addPointCloudNormals<PointT,pcl::Normal>(boost::make_shared<pcl::PointCloud<PointT> >(prev_cloud_), boost::make_shared<pcl::PointCloud<pcl::Normal> >(prev_normals_), 10, 0.05, "normals"); vis_->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "normals"); } else { vis_->removePointCloud ("normals"); } displayEuclideanClusters (prev_clusters_,vis_); previous_data_size_ = prev_regions_.size (); previous_clusters_size_ = prev_clusters_.size (); data_modified_ = false; } } ui_->qvtk_widget->update(); }
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 (); }