108 lines
3.7 KiB
C++
108 lines
3.7 KiB
C++
#include <iostream>
|
|
#include <vector>
|
|
#include <pcl/point_types.h>
|
|
#include <pcl/io/pcd_io.h>
|
|
#include <pcl/search/search.h>
|
|
#include <pcl/search/kdtree.h>
|
|
#include <pcl/features/normal_3d.h>
|
|
#include <pcl/visualization/cloud_viewer.h>
|
|
#include <pcl/filters/passthrough.h>
|
|
#include <pcl/segmentation/region_growing.h>
|
|
#include "o3d3xx_camera.h"
|
|
#include "o3d3xx_framegrabber.h"
|
|
#include "o3d3xx_image.h"
|
|
|
|
using namespace std;
|
|
|
|
int
|
|
main (int argc, char** argv)
|
|
{
|
|
//logging method
|
|
o3d3xx::Logging::Init();
|
|
//initialise camera constructor expects IP address
|
|
o3d3xx::Camera::Ptr cam = std::make_shared<o3d3xx::Camera>("192.168.1.69");
|
|
//create buffer to fetch image
|
|
o3d3xx::ImageBuffer::Ptr img = std::make_shared<o3d3xx::ImageBuffer>();
|
|
//framegrabber
|
|
o3d3xx::FrameGrabber::Ptr fg =
|
|
std::make_shared<o3d3xx::FrameGrabber>(
|
|
cam, o3d3xx::IMG_AMP|o3d3xx::IMG_RDIS|o3d3xx::IMG_CART);
|
|
|
|
//get frame from camera
|
|
if (! fg->WaitForFrame(img.get(), 2000))
|
|
{
|
|
std::cerr << "Timeout waiting for camera!" << std::endl;
|
|
return -1;
|
|
}
|
|
pcl::PCDWriter writer;
|
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
|
|
pcl::PointCloud<pcl::PointXYZI>::Ptr cloudIn (new pcl::PointCloud<pcl::PointXYZI>);
|
|
cloudIn = img->Cloud();
|
|
writer.write<pcl::PointXYZI>("region_growing_tutorial.pcd", *cloudIn, false);
|
|
|
|
if ( pcl::io::loadPCDFile <pcl::PointXYZ> ("region_growing_tutorial.pcd", *cloud) == -1)
|
|
{
|
|
std::cout << "Cloud reading failed." << std::endl;
|
|
return (-1);
|
|
}
|
|
vector<int> temp;
|
|
pcl::removeNaNFromPointCloud(*cloud,*cloud, temp);
|
|
cout << temp.size() << endl;
|
|
cout << cloud->size() << endl;
|
|
|
|
pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> > (new pcl::search::KdTree<pcl::PointXYZ>);
|
|
pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
|
|
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
|
|
normal_estimator.setSearchMethod (tree);
|
|
normal_estimator.setInputCloud (cloud);
|
|
normal_estimator.setKSearch (50);
|
|
normal_estimator.compute (*normals);
|
|
|
|
pcl::IndicesPtr indices (new std::vector <int>);
|
|
pcl::PassThrough<pcl::PointXYZ> pass;
|
|
pass.setInputCloud (cloud);
|
|
pass.setFilterFieldName ("z");
|
|
pass.setFilterLimits (0.0, 1.0);
|
|
pass.filter (*indices);
|
|
|
|
pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
|
|
reg.setMinClusterSize (50);
|
|
reg.setMaxClusterSize (1000000);
|
|
reg.setSearchMethod (tree);
|
|
reg.setNumberOfNeighbours (30);
|
|
reg.setInputCloud (cloud);
|
|
//reg.setIndices (indices);
|
|
reg.setInputNormals (normals);
|
|
reg.setSmoothnessThreshold (3.0 / 180.0 * M_PI);// graden naar radial
|
|
reg.setCurvatureThreshold (1.0);
|
|
|
|
std::vector <pcl::PointIndices> clusters;
|
|
reg.extract (clusters);
|
|
|
|
std::cout << "Number of clusters is equal to " << clusters.size () << std::endl;
|
|
std::cout << "First cluster has " << clusters[0].indices.size () << " points." << endl;
|
|
std::cout << "These are the indices of the points of the initial" <<
|
|
std::endl << "cloud that belong to the first cluster:" << std::endl;
|
|
int counter = 0;
|
|
while (counter < clusters[0].indices.size ())
|
|
{
|
|
std::cout << clusters[0].indices[counter] << ", ";
|
|
counter++;
|
|
if (counter % 10 == 0)
|
|
std::cout << std::endl;
|
|
}
|
|
std::cout << std::endl;
|
|
|
|
pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud ();
|
|
pcl::visualization::PCLVisualizer viewer ("Cluster viewer");
|
|
viewer.addPointCloud<pcl::PointXYZRGB>(colored_cloud,"cloud");
|
|
//viewer.addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal>(colored_cloud, normals, 10, 0.05, "normals",0);
|
|
while (!viewer.wasStopped ())
|
|
{
|
|
viewer.spinOnce(100);
|
|
}
|
|
|
|
return (0);
|
|
}
|