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 (); } }
int main(int argc, char *argv[]) { std::string training_input_dir(argv[1]), trained_data_dir(argv[2]); //detector od::ODDetectorMultiAlgo *detector = new od::ODDetectorMultiAlgo(trained_data_dir); detector->setTrainingInputLocation(training_input_dir); detector->init(); //GUI and feedback od::ODScenePointCloud<pcl::PointXYZRGBA> *frame; pcl::visualization::PCLVisualizer vis ("kinect"); size_t previous_cluster_size = 0; od::ODFrameGenerator<od::ODScenePointCloud<pcl::PointXYZRGBA>, od::GENERATOR_TYPE_DEVICE> frameGenerator; while(frameGenerator.isValid()) { frame = frameGenerator.getNextFrame(); //remove previous point clouds and text and add new ones in the visualizer vis.removePointCloud ("frame"); vis.addPointCloud<pcl::PointXYZRGBA> (frame->getPointCloud(), "frame"); for (size_t i = 0; i < previous_cluster_size; i++) { std::stringstream cluster_name; cluster_name << "cluster_" << i; vis.removePointCloud (cluster_name.str ()); vis.removeText3D (cluster_name.str() + "_txt"); cluster_name << "_ply_model_"; vis.removeShape (cluster_name.str ()); } //Detect od::ODDetections3D * detections = detector->detectOmni(frame); //add all the detections in the visualizer with its id as text for (size_t i = 0; i < detections->size (); i++) { std::stringstream cluster_name; cluster_name << "cluster_" << i; pcl::visualization::PointCloudColorHandlerRandom<pcl::PointXYZ> random_handler (detections->at(i)->getMetainfoCluster()); vis.addPointCloud<pcl::PointXYZ> (detections->at(i)->getMetainfoCluster(), random_handler, cluster_name.str ()); pcl::PointXYZ pos; pos.x = detections->at(i)->getLocation()[0]; pos.y = detections->at(i)->getLocation()[1]; pos.z = detections->at(i)->getLocation()[2]; vis.addText3D (detections->at(i)->getId(), pos, 0.015f, 1, 0, 1, cluster_name.str() + "_txt", 0); } previous_cluster_size = detections->size (); vis.spinOnce (); } return 0; }