static THD_FUNCTION(frame_thread, arg) { (void)arg; while(true) { chBSemWait(&frame_thread_sem); frame_process(); } }
void segmentAndClassify (typename pcl::rec_3d_framework::GlobalNNPipeline<DistT, PointT, FeatureT> & global) { //get point cloud from the kinect, segment it and classify it OpenNIFrameSource::OpenNIFrameSource camera; OpenNIFrameSource::PointCloudPtr frame; pcl::visualization::PCLVisualizer vis ("kinect"); //keyboard callback to stop getting frames and finalize application boost::function<void (const pcl::visualization::KeyboardEvent&)> keyboard_cb = boost::bind (&OpenNIFrameSource::OpenNIFrameSource::onKeyboardEvent, &camera, _1); vis.registerKeyboardCallback (keyboard_cb); size_t previous_cluster_size = 0; size_t previous_categories_size = 0; float Z_DIST_ = 1.25f; float text_scale = 0.015f; while (camera.isActive ()) { pcl::ScopeTime frame_process ("Global frame processing ------------- "); frame = camera.snap (); pcl::PointCloud<pcl::PointXYZ>::Ptr xyz_points (new pcl::PointCloud<pcl::PointXYZ>); pcl::copyPointCloud (*frame, *xyz_points); //Step 1 -> Segment pcl::apps::DominantPlaneSegmentation<pcl::PointXYZ> dps; dps.setInputCloud (xyz_points); dps.setMaxZBounds (Z_DIST_); dps.setObjectMinHeight (0.005); dps.setMinClusterSize (1000); dps.setWSize (9); dps.setDistanceBetweenClusters (0.1f); std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters; std::vector<pcl::PointIndices> indices; dps.setDownsamplingSize (0.02f); dps.compute_fast (clusters); dps.getIndicesClusters (indices); Eigen::Vector4f table_plane_; Eigen::Vector3f normal_plane_ = Eigen::Vector3f (table_plane_[0], table_plane_[1], table_plane_[2]); dps.getTableCoefficients (table_plane_); vis.removePointCloud ("frame"); vis.addPointCloud<OpenNIFrameSource::PointT> (frame, "frame"); for (size_t i = 0; i < previous_cluster_size; i++) { std::stringstream cluster_name; cluster_name << "cluster_" << i; vis.removePointCloud (cluster_name.str ()); cluster_name << "_ply_model_"; vis.removeShape (cluster_name.str ()); } for (size_t i = 0; i < previous_categories_size; i++) { std::stringstream cluster_text; cluster_text << "cluster_" << i << "_text"; vis.removeText3D (cluster_text.str ()); } previous_categories_size = 0; float dist_ = 0.03f; for (size_t i = 0; i < clusters.size (); i++) { std::stringstream cluster_name; cluster_name << "cluster_" << i; pcl::visualization::PointCloudColorHandlerRandom<pcl::PointXYZ> random_handler (clusters[i]); vis.addPointCloud<pcl::PointXYZ> (clusters[i], random_handler, cluster_name.str ()); global.setInputCloud (xyz_points); global.setIndices (indices[i].indices); global.classify (); std::vector < std::string > categories; std::vector<float> conf; global.getCategory (categories); global.getConfidence (conf); std::string category = categories[0]; Eigen::Vector4f centroid; pcl::compute3DCentroid (*xyz_points, indices[i].indices, centroid); for (size_t kk = 0; kk < categories.size (); kk++) { pcl::PointXYZ pos; pos.x = centroid[0] + normal_plane_[0] * static_cast<float> (kk + 1) * dist_; pos.y = centroid[1] + normal_plane_[1] * static_cast<float> (kk + 1) * dist_; pos.z = centroid[2] + normal_plane_[2] * static_cast<float> (kk + 1) * dist_; std::ostringstream prob_str; prob_str.precision (1); prob_str << categories[kk] << " [" << conf[kk] << "]"; std::stringstream cluster_text; cluster_text << "cluster_" << previous_categories_size << "_text"; vis.addText3D (prob_str.str (), pos, text_scale, 1, 0, 1, cluster_text.str (), 0); previous_categories_size++; } } previous_cluster_size = clusters.size (); vis.spinOnce (); } }