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