Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Remove magic numbers from organized_segmentation_demo app #3108

Merged
merged 9 commits into from
Jun 3, 2019
Merged

Remove magic numbers from organized_segmentation_demo app #3108

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
bc996a7
change new to boost::make_shared
May 28, 2019
883d3fa
remove useless spaces
May 28, 2019
b40b42d
remove values set identically to default values
May 28, 2019
b799bf4
remove magic number
May 28, 2019
4616cb7
fix unsigned-signed comparison
May 28, 2019
51654c9
amend 4616cb7
May 28, 2019
192d442
revert bc996a7df45ebb6125bf6946b1d1a14a460e8831
May 28, 2019
31c8671
Revert "remove values set identically to default values"
Jun 3, 2019
d16c3f9
comment on values set identically to default values
Jun 3, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
30 changes: 15 additions & 15 deletions apps/include/pcl/apps/organized_segmentation_demo.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,15 +1,15 @@
/*
* Software License Agreement (BSD License)
*
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2012-, Open Perception, Inc.
*
*
* All rights reserved.
*
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
Expand All @@ -19,7 +19,7 @@
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
Expand Down Expand Up @@ -89,7 +89,7 @@ class OrganizedSegmentationDemo : public QMainWindow
typedef pcl::PointCloud<PointT> Cloud;
typedef Cloud::Ptr CloudPtr;
typedef Cloud::ConstPtr CloudConstPtr;


OrganizedSegmentationDemo(pcl::Grabber& grabber);

Expand All @@ -98,9 +98,9 @@ class OrganizedSegmentationDemo : public QMainWindow
if(grabber_.isRunning())
grabber_.stop();
}

void cloud_cb (const CloudConstPtr& cloud);

protected:
pcl::visualization::PCLVisualizer::Ptr vis_;
pcl::Grabber& grabber_;
Expand All @@ -113,9 +113,9 @@ class OrganizedSegmentationDemo : public QMainWindow
pcl::PointCloud<pcl::Normal> prev_normals_;
std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > prev_regions_;
float* prev_distance_map_;

pcl::PointCloud<PointT>::CloudVectorType prev_clusters_;

pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;

Expand Down Expand Up @@ -148,16 +148,16 @@ class OrganizedSegmentationDemo : public QMainWindow
void useEuclideanComparatorPressed ();
void useRGBComparatorPressed ();
void useEdgeAwareComparatorPressed ();

void displayCurvaturePressed ();
void displayDistanceMapPressed ();
void displayNormalsPressed ();

void disableRefinementPressed ()
{
use_planar_refinement_ = false;
}

void usePlanarRefinementPressed ()
{
use_planar_refinement_ = true;
Expand All @@ -172,7 +172,7 @@ class OrganizedSegmentationDemo : public QMainWindow
{
use_clustering_ = true;
}


private Q_SLOTS:
void
Expand Down
87 changes: 42 additions & 45 deletions apps/src/organized_segmentation_demo.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,15 +12,15 @@
#include <vtkRenderWindow.h>

void
displayPlanarRegions (std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > &regions,
displayPlanarRegions (std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > &regions,
pcl::visualization::PCLVisualizer::Ptr viewer)
{
char name[1024];
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::PointCloud<PointT>::Ptr contour (new pcl::PointCloud<PointT>);
pcl::PointCloud<PointT>::Ptr contour = boost::make_shared<pcl::PointCloud<PointT> > ();

for (size_t i = 0; i < regions.size (); i++)
{
Expand All @@ -32,7 +32,7 @@ displayPlanarRegions (std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allo
centroid[2] + (0.5f * model[2]));
sprintf (name, "normal_%d", unsigned (i));
viewer->addArrow (pt2, pt1, 1.0, 0, 0, false, name);

contour->points = regions[i].getContour ();
sprintf (name, "plane_%02d", int (i));
pcl::visualization::PointCloudColorHandlerCustom <PointT> color (contour, red[i%6], grn[i%6], blu[i%6]);
Expand All @@ -43,7 +43,7 @@ displayPlanarRegions (std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allo
}

void
displayEuclideanClusters (const pcl::PointCloud<PointT>::CloudVectorType &clusters,
displayEuclideanClusters (const pcl::PointCloud<PointT>::CloudVectorType &clusters,
pcl::visualization::PCLVisualizer::Ptr viewer)
{
char name[1024];
Expand All @@ -54,7 +54,7 @@ displayEuclideanClusters (const pcl::PointCloud<PointT>::CloudVectorType &cluste
for (size_t i = 0; i < clusters.size (); i++)
{
sprintf (name, "cluster_%d" , int (i));
pcl::PointCloud<PointT>::ConstPtr cluster_cloud (new pcl::PointCloud<PointT> (clusters[i]));
pcl::PointCloud<PointT>::ConstPtr cluster_cloud = boost::make_shared<pcl::PointCloud<PointT> > (clusters[i]);
pcl::visualization::PointCloudColorHandlerCustom<PointT> color0(cluster_cloud,red[i%6],grn[i%6],blu[i%6]);
if (!viewer->updatePointCloud (cluster_cloud,color0,name))
viewer->addPointCloud (cluster_cloud,color0,name);
Expand Down Expand Up @@ -82,7 +82,7 @@ displayCurvature (pcl::PointCloud<PointT>& cloud, pcl::PointCloud<pcl::Normal>&
curvature_cloud.points[i].b = 0;
}
}

if (!viewer->updatePointCloud (curvature_cloud.makeShared (), "curvature"))
viewer->addPointCloud (curvature_cloud.makeShared (), "curvature");
}
Expand All @@ -106,7 +106,7 @@ displayDistanceMap (pcl::PointCloud<PointT>& cloud, float* distance_map, pcl::vi
distance_map_cloud.points[i].b = 0;
}
}

if (!viewer->updatePointCloud (distance_map_cloud.makeShared (), "distance_map"))
viewer->addPointCloud (distance_map_cloud.makeShared (), "distance_map");
}
Expand All @@ -119,11 +119,11 @@ removePreviousDataFromScreen (size_t prev_models_size, size_t prev_clusters_size
{
sprintf (name, "normal_%d", unsigned (i));
viewer->removeShape (name);

sprintf (name, "plane_%02d", int (i));
viewer->removePointCloud (name);
}

for (size_t i = 0; i < prev_clusters_size; i++)
{
sprintf (name, "cluster_%d", int (i));
Expand All @@ -137,7 +137,7 @@ compareClusterToRegion (pcl::PlanarRegion<PointT>& region, pcl::PointCloud<Point
Eigen::Vector4f model = region.getCoefficients ();
pcl::PointCloud<PointT> poly;
poly.points = region.getContour ();

for (const auto &point : cluster.points)
{
double ptp_dist = fabs (model[0] * point.x +
Expand All @@ -155,7 +155,7 @@ bool
comparePointToRegion (PointT& pt, pcl::ModelCoefficients& model, pcl::PointCloud<PointT>& poly)
{
//bool dist_ok;

double ptp_dist = fabs (model.values[0] * pt.x +
model.values[1] * pt.y +
model.values[2] * pt.z +
Expand All @@ -175,7 +175,7 @@ comparePointToRegion (PointT& pt, pcl::ModelCoefficients& model, pcl::PointCloud
PointT projected_pt;
projected_pt.x = projected[0];
projected_pt.y = projected[1];
projected_pt.z = projected[2];
projected_pt.z = projected[2];

PCL_INFO ("pt: %lf %lf %lf\n", projected_pt.x, projected_pt.y, projected_pt.z);

Expand All @@ -189,7 +189,7 @@ comparePointToRegion (PointT& pt, pcl::ModelCoefficients& model, pcl::PointCloud
PCL_INFO ("not inside!\n");
return false;
}

}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Expand All @@ -203,17 +203,17 @@ OrganizedSegmentationDemo::OrganizedSegmentationDemo (pcl::Grabber& grabber) : g

ui_ = new Ui::MainWindow;
ui_->setupUi (this);

this->setWindowTitle ("PCL Organized Connected Component Segmentation Demo");
vis_.reset (new pcl::visualization::PCLVisualizer ("", false));
vis_ = boost::make_shared<pcl::visualization::PCLVisualizer> ("", false);
ui_->qvtk_widget->SetRenderWindow (vis_->getRenderWindow ());
vis_->setupInteractor (ui_->qvtk_widget->GetInteractor (), ui_->qvtk_widget->GetRenderWindow ());
vis_->getInteractorStyle ()->setKeyboardModifier (pcl::visualization::INTERACTOR_KB_MOD_SHIFT);
ui_->qvtk_widget->update ();

boost::function<void (const CloudConstPtr&)> f = boost::bind (&OrganizedSegmentationDemo::cloud_cb, this, _1);
boost::signals2::connection c = grabber_.registerCallback(f);

connect (ui_->captureButton, SIGNAL(clicked()), this, SLOT(toggleCapturePressed()));

connect (ui_->euclideanComparatorButton, SIGNAL (clicked ()), this, SLOT (useEuclideanComparatorPressed ()));
Expand Down Expand Up @@ -242,40 +242,37 @@ OrganizedSegmentationDemo::OrganizedSegmentationDemo (pcl::Grabber& grabber) : g

use_planar_refinement_ = true;
use_clustering_ = false;


// Set up Normal Estimation
//ne.setNormalEstimationMethod (ne.SIMPLE_3D_GRADIENT);
ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
ne.setMaxDepthChangeFactor (0.02f);
ne.setNormalSmoothingSize (20.0f);

plane_comparator_.reset (new pcl::PlaneCoefficientComparator<PointT, pcl::Normal> ());
euclidean_comparator_.reset (new pcl::EuclideanPlaneCoefficientComparator<PointT, pcl::Normal> ());
rgb_comparator_.reset (new pcl::RGBPlaneCoefficientComparator<PointT, pcl::Normal> ());
edge_aware_comparator_.reset (new pcl::EdgeAwarePlaneComparator<PointT, pcl::Normal> ());
euclidean_cluster_comparator_ = pcl::EuclideanClusterComparator<PointT, pcl::Label>::Ptr (new pcl::EuclideanClusterComparator<PointT, pcl::Label> ());
plane_comparator_ = boost::make_shared<pcl::PlaneCoefficientComparator<PointT, pcl::Normal> > ();
euclidean_comparator_ = boost::make_shared<pcl::EuclideanPlaneCoefficientComparator<PointT, pcl::Normal> > ();
rgb_comparator_ = boost::make_shared<pcl::RGBPlaneCoefficientComparator<PointT, pcl::Normal> > ();
edge_aware_comparator_ = boost::make_shared<pcl::EdgeAwarePlaneComparator<PointT, pcl::Normal> > ();
euclidean_cluster_comparator_ = boost::make_shared<pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label> > ();

// Set up Organized Multi Plane Segmentation
mps.setMinInliers (10000);
mps.setAngularThreshold (pcl::deg2rad (3.0)); //3 degrees
mps.setDistanceThreshold (0.02); //2cm

mps.setMinInliers (10000u);


PCL_INFO ("starting grabber\n");
grabber_.start ();
}

void
void
OrganizedSegmentationDemo::cloud_cb (const CloudConstPtr& cloud)
{
{
if (!capture_)
return;
QMutexLocker locker (&mtx_);
FPS_CALC ("computation");

// Estimate Normals
pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<pcl::Normal>::Ptr normal_cloud = boost::make_shared<pcl::PointCloud<pcl::Normal> > ();
ne.setInputCloud (cloud);
ne.compute (*normal_cloud);
float* distance_map = ne.getDistanceMap ();
Expand All @@ -287,8 +284,8 @@ OrganizedSegmentationDemo::cloud_cb (const CloudConstPtr& cloud)
double mps_start = pcl::getTime ();
std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
std::vector<pcl::ModelCoefficients> model_coefficients;
std::vector<pcl::PointIndices> inlier_indices;
pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>);
std::vector<pcl::PointIndices> inlier_indices;
pcl::PointCloud<pcl::Label>::Ptr labels = boost::make_shared<pcl::PointCloud<pcl::Label> > ();
std::vector<pcl::PointIndices> label_indices;
std::vector<pcl::PointIndices> boundary_indices;
mps.setInputNormals (normal_cloud);
Expand All @@ -309,9 +306,9 @@ OrganizedSegmentationDemo::cloud_cb (const CloudConstPtr& cloud)

if (use_clustering_ && !regions.empty ())
{
pcl::EuclideanClusterComparator<PointT, pcl::Label>::ExcludeLabelSetPtr plane_labels (new pcl::EuclideanClusterComparator<PointT, pcl::Label>::ExcludeLabelSet);
pcl::EuclideanClusterComparator<PointT, pcl::Label>::ExcludeLabelSetPtr plane_labels = boost::make_shared<pcl::EuclideanClusterComparator<PointT, pcl::Label>::ExcludeLabelSet> ();
for (size_t i = 0; i < label_indices.size (); ++i)
if (label_indices[i].indices.size () > 10000)
if (label_indices[i].indices.size () > mps.getMinInliers())
plane_labels->insert (i);

euclidean_cluster_comparator_->setInputCloud (cloud);
Expand All @@ -324,21 +321,21 @@ OrganizedSegmentationDemo::cloud_cb (const CloudConstPtr& cloud)
pcl::OrganizedConnectedComponentSegmentation<PointT,pcl::Label> euclidean_segmentation (euclidean_cluster_comparator_);
euclidean_segmentation.setInputCloud (cloud);
euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);

for (const auto &euclidean_label_index : euclidean_label_indices)
{
if (euclidean_label_index.indices.size () > 1000)
if (euclidean_label_index.indices.size () > 1000u)
{
pcl::PointCloud<PointT> cluster;
pcl::copyPointCloud (*cloud, euclidean_label_index.indices,cluster);
clusters.push_back (cluster);
}
}
}

PCL_INFO ("Got %d euclidean clusters!\n", clusters.size ());
}
}

{
{
QMutexLocker vis_locker (&vis_mtx_);
prev_cloud_ = *cloud;
prev_normals_ = *normal_cloud;
Expand All @@ -349,7 +346,7 @@ OrganizedSegmentationDemo::cloud_cb (const CloudConstPtr& cloud)
}
}

void
void
OrganizedSegmentationDemo::timeoutSlot ()
{
{
Expand Down Expand Up @@ -385,15 +382,15 @@ OrganizedSegmentationDemo::timeoutSlot ()
{
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();
}

Expand Down Expand Up @@ -451,7 +448,7 @@ int
main (int argc, char ** argv)
{
QApplication app(argc, argv);

//PCL_INFO ("Creating PCD Grabber\n");
//std::vector<std::string> pcd_files;
//boost::filesystem::directory_iterator end_itr;
Expand All @@ -466,7 +463,7 @@ main (int argc, char ** argv)
//PCL_INFO ("PCD Grabber created\n");

pcl::OpenNIGrabber grabber ("#1");

OrganizedSegmentationDemo seg_demo (grabber);
seg_demo.show();
return (QApplication::exec ());
Expand Down