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 ();
}
}