void initViewer(pcl::visualization::PCLVisualizer &viewer) {
viewer.setBackgroundColor(0, 0, 0);
viewer.addCoordinateSystem(1.0, "reference");
viewer.initCameraParameters();
viewer.setRepresentationToPointsForAllActors();
viewer.setCameraPosition(0, 0, -1, 0, 0, 0, 0, -1, 0);
viewer.registerKeyboardCallback(keyboardCallback);
}
int main(int argc, char* argv[]) {
if(argc < 3) {
PCL_ERROR("run as ./project /path/to/cloud1/ /path/to/cloud2/ [pcd format]");
return -1;
}
string fCloud1 = "";
string fCloud2 = "";
string models = "";
string training = "";
int NN;
pcl::console::parse_argument(argc, argv, "-cloud1", fCloud1);
pcl::console::parse_argument(argc, argv, "-cloud2", fCloud2);
pcl::console::parse_argument(argc, argv, "-models", models);
pcl::console::parse_argument(argc, argv, "-training", training);
pcl::console::parse_argument(argc, argv, "-nn", NN);
moi.setVerbose(true);
classificator.setModelsDir(models);
classificator.setTrainingDir(training);
classificator.setNN(NN);
classificator.setup();
viewer.registerKeyboardCallback(&keyboard_cb, NULL);
viewer.setBackgroundColor(0, 0, 0);
viewer.initCameraParameters();
if(fCloud1 != "" && fCloud2 != "") {
if(pcl::io::loadPCDFile<pcl::PointXYZ>(fCloud1, *cloud1) == -1) {
PCL_ERROR("Cloud1 reading failed\n");
return(-1);
}
if(pcl::io::loadPCDFile<pcl::PointXYZ>(fCloud2, *cloud2) == -1) {
PCL_ERROR("Cloud2 reading failed\n");
return(-1);
}
findObjectsAndClassify();
} else {
keyboardCbLock = false;
printInstructions();
}
while(!viewer.wasStopped()) {
viewer.spinOnce(100);
boost::this_thread::sleep (boost::posix_time::milliseconds(100));
}
return 0;
}
PCDOrganizedMultiPlaneSegmentation (typename pcl::PointCloud<PointT>::ConstPtr cloud_, bool refine)
: viewer ("Viewer")
, cloud (cloud_)
, refine_ (refine)
, threshold_ (0.02f)
, depth_dependent_ (true)
, polygon_refinement_ (false)
{
viewer.setBackgroundColor (0, 0, 0);
//viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud");
//viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.15, "cloud");
viewer.addCoordinateSystem (1.0, "global");
viewer.initCameraParameters ();
viewer.registerKeyboardCallback(&PCDOrganizedMultiPlaneSegmentation::keyboard_callback, *this, 0);
}
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
float th_dd, int max_search)
{
CloudPtr cloud (new Cloud);
fromROSMsg (*input, *cloud);
pcl::PointCloud<pcl::Normal>::Ptr normal (new pcl::PointCloud<pcl::Normal>);
pcl::IntegralImageNormalEstimation<PointXYZRGBA, pcl::Normal> ne;
ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
ne.setNormalSmoothingSize (10.0f);
ne.setBorderPolicy (ne.BORDER_POLICY_MIRROR);
ne.setInputCloud (cloud);
ne.compute (*normal);
TicToc tt;
tt.tic ();
//OrganizedEdgeBase<PointXYZRGBA, Label> oed;
//OrganizedEdgeFromRGB<PointXYZRGBA, Label> oed;
//OrganizedEdgeFromNormals<PointXYZRGBA, Normal, Label> oed;
OrganizedEdgeFromRGBNormals<PointXYZRGBA, Normal, Label> oed;
oed.setInputNormals (normal);
oed.setInputCloud (cloud);
oed.setDepthDisconThreshold (th_dd);
oed.setMaxSearchNeighbors (max_search);
oed.setEdgeType (oed.EDGELABEL_NAN_BOUNDARY | oed.EDGELABEL_OCCLUDING | oed.EDGELABEL_OCCLUDED | oed.EDGELABEL_HIGH_CURVATURE | oed.EDGELABEL_RGB_CANNY);
PointCloud<Label> labels;
std::vector<PointIndices> label_indices;
oed.compute (labels, label_indices);
print_info ("Detecting all edges... [done, "); print_value ("%g", tt.toc ()); print_info (" ms]\n");
// Make gray point clouds
for (int idx = 0; idx < (int)cloud->points.size (); idx++)
{
uint8_t gray = (cloud->points[idx].r + cloud->points[idx].g + cloud->points[idx].b)/3;
cloud->points[idx].r = cloud->points[idx].g = cloud->points[idx].b = gray;
}
// Display edges in PCLVisualizer
viewer.setSize (640, 480);
viewer.addCoordinateSystem (0.2f);
viewer.addPointCloud (cloud, "original point cloud");
viewer.registerKeyboardCallback(&keyboard_callback);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr occluding_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
occluded_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
nan_boundary_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
high_curvature_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
rgb_edges (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::copyPointCloud (*cloud, label_indices[0].indices, *nan_boundary_edges);
pcl::copyPointCloud (*cloud, label_indices[1].indices, *occluding_edges);
pcl::copyPointCloud (*cloud, label_indices[2].indices, *occluded_edges);
pcl::copyPointCloud (*cloud, label_indices[3].indices, *high_curvature_edges);
pcl::copyPointCloud (*cloud, label_indices[4].indices, *rgb_edges);
const int point_size = 2;
viewer.addPointCloud<pcl::PointXYZRGBA> (nan_boundary_edges, "nan boundary edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "nan boundary edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 0.0f, 1.0f, "nan boundary edges");
viewer.addPointCloud<pcl::PointXYZRGBA> (occluding_edges, "occluding edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluding edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 0.0f, "occluding edges");
viewer.addPointCloud<pcl::PointXYZRGBA> (occluded_edges, "occluded edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluded edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 0.0f, 0.0f, "occluded edges");
viewer.addPointCloud<pcl::PointXYZRGBA> (high_curvature_edges, "high curvature edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "high curvature edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 1.0f, 0.0f, "high curvature edges");
viewer.addPointCloud<pcl::PointXYZRGBA> (rgb_edges, "rgb edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "rgb edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 1.0f, "rgb edges");
while (!viewer.wasStopped ())
{
viewer.spinOnce ();
pcl_sleep(0.1);
}
// Combine point clouds and edge labels
sensor_msgs::PointCloud2 output_edges;
toROSMsg (labels, output_edges);
concatenateFields (*input, output_edges, output);
}
