void PCDReader::Read() {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud_xyz) == -1) //* load the file
{
cout <<"Błąd"<<endl;
}
out_cloud_xyz.write(cloud_xyz);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_xyzrgb (new pcl::PointCloud<pcl::PointXYZRGB>);
if (pcl::io::loadPCDFile<pcl::PointXYZRGB> (filename, *cloud_xyzrgb) == -1) //* load the file
{
cout <<"Błąd"<<endl;
}
out_cloud_xyzrgb.write(cloud_xyzrgb);
pcl::PointCloud<PointXYZSIFT>::Ptr cloud_xyzsift (new pcl::PointCloud<PointXYZSIFT>);
if (pcl::io::loadPCDFile<PointXYZSIFT> (filename, *cloud_xyzsift) == -1) //* load the file
{
cout <<"Błąd"<<endl;
}
out_cloud_xyzsift.write(cloud_xyzsift);
}
void PCDReader::Read() {
CLOG(LTRACE) << "PCDReader::Read\n";
// Try to read the cloud of XYZ points.
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
/*
if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud_xyz) == -1){
CLOG(LWARNING) <<"Cannot read PointXYZ cloud from "<<filename;
}else{
out_cloud_xyz.write(cloud_xyz);
CLOG(LINFO) <<"PointXYZ size: "<<cloud_xyz->size();
CLOG(LINFO) <<"PointXYZ cloud loaded properly from "<<filename;
//return;
}// else
*/
// Try to read the cloud of XYZRGB points.
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_xyzrgb (new pcl::PointCloud<pcl::PointXYZRGB>);
if (pcl::io::loadPCDFile<pcl::PointXYZRGB> (filename, *cloud_xyzrgb) == -1){
CLOG(LWARNING) <<"Cannot read PointXYZRGB cloud from "<<filename;
}else{
out_cloud_xyzrgb.write(cloud_xyzrgb);
CLOG(LINFO) <<"PointXYZRGB cloud loaded properly from "<<filename;
//return;
}// else
// Try to read the cloud of XYZSIFT points.
pcl::PointCloud<PointXYZSIFT>::Ptr cloud_xyzsift (new pcl::PointCloud<PointXYZSIFT>);
if (pcl::io::loadPCDFile<PointXYZSIFT> (filename, *cloud_xyzsift) == -1){
CLOG(LWARNING) <<"Cannot read PointXYZSIFT cloud from "<<filename;
}else{
out_cloud_xyzsift.write(cloud_xyzsift);
CLOG(LINFO) <<"PointXYZSIFT cloud loaded properly from "<<filename;
//return;
}// else
}
template <typename PointT> Eigen::VectorXf
open_ptrack::detection::GroundplaneEstimation<PointT>::compute ()
{
Eigen::VectorXf ground_coeffs;
ground_coeffs.resize(4);
// Manual mode:
if (ground_estimation_mode_ == 0)
{
std::cout << "Manual mode for ground plane estimation." << std::endl;
// Initialize viewer:
pcl::visualization::PCLVisualizer viewer("Pick 3 points");
// Create XYZ cloud:
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_xyzrgb(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointXYZRGB xyzrgb_point;
cloud_xyzrgb->points.resize(cloud_->width * cloud_->height, xyzrgb_point);
cloud_xyzrgb->width = cloud_->width;
cloud_xyzrgb->height = cloud_->height;
cloud_xyzrgb->is_dense = false;
for (int i=0; i<cloud_->height; i++)
{
for (int j=0; j<cloud_->width; j++)
{
cloud_xyzrgb->at(j,i).x = cloud_->at(j,i).x;
cloud_xyzrgb->at(j,i).y = cloud_->at(j,i).y;
cloud_xyzrgb->at(j,i).z = cloud_->at(j,i).z;
}
}
//#if (XYZRGB_CLOUDS)
// pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud_);
// viewer.addPointCloud<pcl::PointXYZRGB> (cloud_, rgb, "input_cloud");
//#else
// viewer.addPointCloud<pcl::PointXYZ> (cloud_, "input_cloud");
//#endif
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> rgb(cloud_xyzrgb, 255, 255, 255);
viewer.addPointCloud<pcl::PointXYZRGB> (cloud_xyzrgb, rgb, "input_cloud");
viewer.setCameraPosition(0,0,-2,0,-1,0,0);
// Add point picking callback to viewer:
struct callback_args_color cb_args;
//#if (XYZRGB_CLOUDS)
// PointCloudPtr clicked_points_3d (new PointCloud);
//#else
// pcl::PointCloud<pcl::PointXYZ>::Ptr clicked_points_3d (new pcl::PointCloud<pcl::PointXYZ>);
//#endif
pcl::PointCloud<pcl::PointXYZRGB>::Ptr clicked_points_3d (new pcl::PointCloud<pcl::PointXYZRGB>);
cb_args.clicked_points_3d = clicked_points_3d;
cb_args.viewerPtr = &viewer;
viewer.registerPointPickingCallback (GroundplaneEstimation::pp_callback, (void*)&cb_args);
// Spin until 'Q' is pressed:
viewer.spin();
viewer.setSize(1,1); // resize viewer in order to make it disappear
viewer.spinOnce();
viewer.close(); // close method does not work
std::cout << "done." << std::endl;
// Keep only the last three clicked points:
while(clicked_points_3d->points.size()>3)
{
clicked_points_3d->points.erase(clicked_points_3d->points.begin());
}
// Ground plane estimation:
std::vector<int> clicked_points_indices;
for (unsigned int i = 0; i < clicked_points_3d->points.size(); i++)
clicked_points_indices.push_back(i);
// pcl::SampleConsensusModelPlane<PointT> model_plane(clicked_points_3d);
pcl::SampleConsensusModelPlane<pcl::PointXYZRGB> model_plane(clicked_points_3d);
model_plane.computeModelCoefficients(clicked_points_indices,ground_coeffs);
std::cout << "Ground plane coefficients: " << ground_coeffs(0) << ", " << ground_coeffs(1) << ", " << ground_coeffs(2) <<
", " << ground_coeffs(3) << "." << std::endl;
}
// Semi-automatic mode:
if (ground_estimation_mode_ == 1)
{
std::cout << "Semi-automatic mode for ground plane estimation." << std::endl;
// Normals computation:
pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
ne.setMaxDepthChangeFactor (0.03f);
ne.setNormalSmoothingSize (20.0f);
pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
ne.setInputCloud (cloud_);
ne.compute (*normal_cloud);
// Multi plane segmentation initialization:
std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
mps.setMinInliers (500);
mps.setAngularThreshold (2.0 * M_PI / 180);
mps.setDistanceThreshold (0.2);
mps.setInputNormals (normal_cloud);
mps.setInputCloud (cloud_);
mps.segmentAndRefine (regions);
std::cout << "Found " << regions.size() << " planar regions." << std::endl;
// Color planar regions with different colors:
pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
colored_cloud = colorRegions(regions);
if (regions.size()>0)
{
// Viewer initialization:
pcl::visualization::PCLVisualizer viewer("PCL Viewer");
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(colored_cloud);
viewer.addPointCloud<pcl::PointXYZRGB> (colored_cloud, rgb, "input_cloud");
viewer.setCameraPosition(0,0,-2,0,-1,0,0);
// Add point picking callback to viewer:
struct callback_args_color cb_args;
typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr clicked_points_3d (new pcl::PointCloud<pcl::PointXYZRGB>);
cb_args.clicked_points_3d = clicked_points_3d;
cb_args.viewerPtr = &viewer;
viewer.registerPointPickingCallback (GroundplaneEstimation::pp_callback, (void*)&cb_args);
std::cout << "Shift+click on a floor point, then press 'Q'..." << std::endl;
// Spin until 'Q' is pressed:
viewer.spin();
viewer.setSize(1,1); // resize viewer in order to make it disappear
viewer.spinOnce();
viewer.close(); // close method does not work
std::cout << "done." << std::endl;
// Find plane closest to clicked point:
unsigned int index = 0;
float min_distance = FLT_MAX;
float distance;
float X = cb_args.clicked_points_3d->points[clicked_points_3d->points.size() - 1].x;
float Y = cb_args.clicked_points_3d->points[clicked_points_3d->points.size() - 1].y;
float Z = cb_args.clicked_points_3d->points[clicked_points_3d->points.size() - 1].z;
for(unsigned int i = 0; i < regions.size(); i++)
{
float a = regions[i].getCoefficients()[0];
float b = regions[i].getCoefficients()[1];
float c = regions[i].getCoefficients()[2];
float d = regions[i].getCoefficients()[3];
distance = (float) (fabs((a*X + b*Y + c*Z + d)))/(sqrtf(a*a+b*b+c*c));
if(distance < min_distance)
{
min_distance = distance;
index = i;
}
}
ground_coeffs[0] = regions[index].getCoefficients()[0];
ground_coeffs[1] = regions[index].getCoefficients()[1];
ground_coeffs[2] = regions[index].getCoefficients()[2];
ground_coeffs[3] = regions[index].getCoefficients()[3];
std::cout << "Ground plane coefficients: " << regions[index].getCoefficients()[0] << ", " << regions[index].getCoefficients()[1] << ", " <<
regions[index].getCoefficients()[2] << ", " << regions[index].getCoefficients()[3] << "." << std::endl;
}
}
// Automatic mode:
if ((ground_estimation_mode_ == 2) || (ground_estimation_mode_ == 3))
{
std::cout << "Automatic mode for ground plane estimation." << std::endl;
// Normals computation:
// pcl::NormalEstimation<PointT, pcl::Normal> ne;
//// ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
//// ne.setMaxDepthChangeFactor (0.03f);
//// ne.setNormalSmoothingSize (20.0f);
// pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
// pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
// ne.setSearchMethod (tree);
// ne.setRadiusSearch (0.2);
// ne.setInputCloud (cloud_);
// ne.compute (*normal_cloud);
pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
ne.setMaxDepthChangeFactor (0.03f);
ne.setNormalSmoothingSize (20.0f);
pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
ne.setInputCloud (cloud_);
ne.compute (*normal_cloud);
// std::cout << "Normals estimated!" << std::endl;
//
// // Multi plane segmentation initialization:
// std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
// pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
// mps.setMinInliers (500);
// mps.setAngularThreshold (2.0 * M_PI / 180);
// mps.setDistanceThreshold (0.2);
// mps.setInputNormals (normal_cloud);
// mps.setInputCloud (cloud_);
// mps.segmentAndRefine (regions);
// Multi plane segmentation initialization:
std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
mps.setMinInliers (500);
mps.setAngularThreshold (2.0 * M_PI / 180);
mps.setDistanceThreshold (0.2);
mps.setInputNormals (normal_cloud);
mps.setInputCloud (cloud_);
mps.segmentAndRefine (regions);
// std::cout << "Found " << regions.size() << " planar regions." << std::endl;
// Removing planes not compatible with camera roll ~= 0:
unsigned int i = 0;
while(i < regions.size())
{ // Check on the normal to the plane:
if(fabs(regions[i].getCoefficients()[1]) < 0.70)
{
regions.erase(regions.begin()+i);
}
else
++i;
}
// Order planar regions according to height (y coordinate):
std::sort(regions.begin(), regions.end(), GroundplaneEstimation::planeHeightComparator);
// Color selected planar region in red:
pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
colored_cloud = colorRegions(regions, 0);
// If at least a valid plane remained:
if (regions.size()>0)
{
ground_coeffs[0] = regions[0].getCoefficients()[0];
ground_coeffs[1] = regions[0].getCoefficients()[1];
ground_coeffs[2] = regions[0].getCoefficients()[2];
ground_coeffs[3] = regions[0].getCoefficients()[3];
std::cout << "Ground plane coefficients: " << regions[0].getCoefficients()[0] << ", " << regions[0].getCoefficients()[1] << ", " <<
regions[0].getCoefficients()[2] << ", " << regions[0].getCoefficients()[3] << "." << std::endl;
// Result visualization:
if (ground_estimation_mode_ == 2)
{
// Viewer initialization:
pcl::visualization::PCLVisualizer viewer("PCL Viewer");
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(colored_cloud);
viewer.addPointCloud<pcl::PointXYZRGB> (colored_cloud, rgb, "input_cloud");
viewer.setCameraPosition(0,0,-2,0,-1,0,0);
// Spin until 'Q' is pressed:
viewer.spin();
viewer.setSize(1,1); // resize viewer in order to make it disappear
viewer.spinOnce();
viewer.close(); // close method does not work
}
}
else
{
std::cout << "ERROR: no valid ground plane found!" << std::endl;
}
}
return ground_coeffs;
}
//this will be our benchmark testing
void PCLWrapper::benchmark(int iterations, int average){
int count = 0;
//generate a set of random points
int width = 100;
int height = 1000;
unsigned short *test1 = (unsigned short *)malloc(width*height*sizeof(unsigned short)); //depth
unsigned char *test2 = (unsigned char *)malloc(width*height*sizeof(unsigned char)*3); //rgb
float *test3 = (float*)malloc(width*height*sizeof(float)*3); //xyz
//begin our testing.
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->width = width;
cloud->height = height;
cloud->points.resize(cloud->width * cloud->height);
//results
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
cloud_filtered->width = width;
cloud_filtered->height = height;
cloud_filtered->points.resize(cloud_filtered->width * cloud_filtered->height);
//unsigned short version
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_xyzrgb(new pcl::PointCloud<pcl::PointXYZRGB>);
cloud_xyzrgb->width = width;
cloud_xyzrgb->height = height;
cloud_xyzrgb->points.resize(cloud_xyzrgb->width * cloud_xyzrgb->height);
//load the input points with some random numbers
for (size_t i = 0; i < cloud->points.size(); ++i) {
cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
}
while (iterations > 0){
struct timeval start, end;
double t1, t2;
static double elapsed_sec = 0;
const int MAX_COUNT = average; //average 10 results before printing
gettimeofday(&start, NULL);
//do some work here.
// Create the filtering object
pcl::PassThrough < pcl::PointXYZ > pass;
pass.setInputCloud(cloud);
pass.setFilterFieldName("z");
pass.setFilterLimits(0.0, 1.0);
//pass.setFilterLimitsNegative (true);
pass.filter(*cloud_filtered);
gettimeofday(&end, NULL);
t1 = start.tv_sec + (start.tv_usec / 1000000.0);
t2 = end.tv_sec + (end.tv_usec / 1000000.0);
elapsed_sec += (t2 - t1);
count++;
if (count >= MAX_COUNT) {
char buf[512];
sprintf(buf, "Number of Points: %d, Runtime: %f (s)\n", width*height, (elapsed_sec) / MAX_COUNT);
elapsed_sec = 0;
count = 0;
__android_log_write(ANDROID_LOG_INFO, "PCL Benchmark:", buf);
}
iterations--;
}
free(test1);
free(test2);
free(test3);
}
