void callback(const sensor_msgs::PointCloud2ConstPtr& cloud)
{
ros::Time whole_start = ros::Time::now();
ros::Time declare_types_start = ros::Time::now();
// filter
pcl::VoxelGrid<sensor_msgs::PointCloud2> voxel_grid;
pcl::PassThrough<sensor_msgs::PointCloud2> pass;
pcl::ExtractIndices<pcl::PointXYZ> extract_indices;
pcl::ExtractIndices<pcl::Normal> extract_normals;
// Normal estimation
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimation;
pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> segmentation_from_normals;
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZ> ());
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree3 (new pcl::search::KdTree<pcl::PointXYZ> ());
// The plane and sphere coefficients
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients ());
pcl::ModelCoefficients::Ptr coefficients_cylinder (new pcl::ModelCoefficients ());
pcl::ModelCoefficients::Ptr coefficients_sphere (new pcl::ModelCoefficients ());
// The plane and sphere inliers
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices ());
pcl::PointIndices::Ptr inliers_cylinder (new pcl::PointIndices ());
pcl::PointIndices::Ptr inliers_sphere (new pcl::PointIndices ());
// The point clouds
sensor_msgs::PointCloud2::Ptr voxelgrid_filtered (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr plane_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr rest_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr rest_cloud_filtered (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr cylinder_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr sphere_output_cloud (new sensor_msgs::PointCloud2);
// The PointCloud
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr remove_transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_output (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_output (new pcl::PointCloud<pcl::PointXYZ>);
// The cloud normals
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ()); // for plane
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal> ()); // for cylinder
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals3 (new pcl::PointCloud<pcl::Normal> ()); // for sphere
ros::Time declare_types_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Voxel grid Filtering
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// // Create VoxelGrid filtering
// voxel_grid.setInputCloud (cloud);
// voxel_grid.setLeafSize (0.01, 0.01, 0.01);
// voxel_grid.filter (*voxelgrid_filtered);
//
// // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
// pcl::fromROSMsg (*voxelgrid_filtered, *transformed_cloud);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Passthrough Filtering
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// pass through filter
// pass.setInputCloud (cloud);
// pass.setFilterFieldName ("z");
// pass.setFilterLimits (0, 1.5);
// pass.filter (*cloud_filtered);
//
// // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
// pcl::fromROSMsg (*cloud_filtered, *transformed_cloud);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Estimate point normals
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// ros::Time estimate_start = ros::Time::now();
//
// // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
// pcl::fromROSMsg (*cloud, *transformed_cloud);
//
// // Estimate point normals
// normal_estimation.setSearchMethod (tree);
// normal_estimation.setInputCloud (transformed_cloud);
// normal_estimation.setKSearch (50);
// normal_estimation.compute (*cloud_normals);
//
// ros::Time estimate_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Create and processing the plane extraction
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// ros::Time plane_start = ros::Time::now();
//
// // Create the segmentation object for the planar model and set all the parameters
// segmentation_from_normals.setOptimizeCoefficients (true);
// segmentation_from_normals.setModelType (pcl::SACMODEL_NORMAL_PLANE);
// segmentation_from_normals.setNormalDistanceWeight (0.1);
// segmentation_from_normals.setMethodType (pcl::SAC_RANSAC);
// segmentation_from_normals.setMaxIterations (100);
// segmentation_from_normals.setDistanceThreshold (0.03);
// segmentation_from_normals.setInputCloud (transformed_cloud);
// segmentation_from_normals.setInputNormals (cloud_normals);
//
// // Obtain the plane inliers and coefficients
// segmentation_from_normals.segment (*inliers_plane, *coefficients_plane);
// //std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;
//
// // Extract the planar inliers from the input cloud
// extract_indices.setInputCloud (transformed_cloud);
// extract_indices.setIndices (inliers_plane);
// extract_indices.setNegative (false);
// extract_indices.filter (*cloud_plane);
//
// pcl::toROSMsg (*cloud_plane, *plane_output_cloud);
// plane_pub.publish(plane_output_cloud);
//
// ros::Time plane_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time plane_start = ros::Time::now();
pcl::fromROSMsg (*cloud, *transformed_cloud);
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (transformed_cloud);
seg.segment (*inliers_plane, *coefficients_plane);
extract_indices.setInputCloud(transformed_cloud);
extract_indices.setIndices(inliers_plane);
extract_indices.setNegative(false);
extract_indices.filter(*cloud_plane);
pcl::toROSMsg (*cloud_plane, *plane_output_cloud);
plane_pub.publish(plane_output_cloud);
ros::Time plane_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Extract rest plane and passthrough filtering
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time rest_pass_start = ros::Time::now();
// Create the filtering object
// Remove the planar inliers, extract the rest
extract_indices.setNegative (true);
extract_indices.filter (*remove_transformed_cloud);
transformed_cloud.swap (remove_transformed_cloud);
// publish result of Removal the planar inliers, extract the rest
pcl::toROSMsg (*transformed_cloud, *rest_output_cloud);
rest_pub.publish(rest_output_cloud);
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
// pcl::fromROSMsg (*rest_output_cloud, *cylinder_cloud);
// pass through filter
pass.setInputCloud (rest_output_cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 2.5);
pass.filter (*rest_cloud_filtered);
ros::Time rest_pass_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* for cylinder features
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromROSMsg (*rest_cloud_filtered, *cylinder_cloud);
// Estimate point normals
normal_estimation.setSearchMethod (tree2);
normal_estimation.setInputCloud (cylinder_cloud);
normal_estimation.setKSearch (50);
normal_estimation.compute (*cloud_normals2);
// Create the segmentation object for sphere segmentation and set all the paopennirameters
segmentation_from_normals.setOptimizeCoefficients (true);
segmentation_from_normals.setModelType (pcl::SACMODEL_CYLINDER);
segmentation_from_normals.setMethodType (pcl::SAC_RANSAC);
segmentation_from_normals.setNormalDistanceWeight (0.1);
segmentation_from_normals.setMaxIterations (10000);
segmentation_from_normals.setDistanceThreshold (0.05);
segmentation_from_normals.setRadiusLimits (0, 0.5);
segmentation_from_normals.setInputCloud (cylinder_cloud);
segmentation_from_normals.setInputNormals (cloud_normals2);
// Obtain the sphere inliers and coefficients
segmentation_from_normals.segment (*inliers_cylinder, *coefficients_cylinder);
//std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;
// Publish the sphere cloud
extract_indices.setInputCloud (cylinder_cloud);
extract_indices.setIndices (inliers_cylinder);
extract_indices.setNegative (false);
extract_indices.filter (*cylinder_output);
if (cylinder_output->points.empty ())
std::cerr << "Can't find the cylindrical component." << std::endl;
pcl::toROSMsg (*cylinder_output, *cylinder_output_cloud);
cylinder_pub.publish(cylinder_output_cloud);
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* for sphere features
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time sphere_start = ros::Time::now();
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromROSMsg (*rest_cloud_filtered, *sphere_cloud);
// Estimate point normals
normal_estimation.setSearchMethod (tree3);
normal_estimation.setInputCloud (sphere_cloud);
normal_estimation.setKSearch (50);
normal_estimation.compute (*cloud_normals3);
// Create the segmentation object for sphere segmentation and set all the paopennirameters
segmentation_from_normals.setOptimizeCoefficients (true);
//segmentation_from_normals.setModelType (pcl::SACMODEL_SPHERE);
segmentation_from_normals.setModelType (pcl::SACMODEL_SPHERE);
segmentation_from_normals.setMethodType (pcl::SAC_RANSAC);
segmentation_from_normals.setNormalDistanceWeight (0.1);
segmentation_from_normals.setMaxIterations (10000);
segmentation_from_normals.setDistanceThreshold (0.05);
segmentation_from_normals.setRadiusLimits (0, 0.2);
segmentation_from_normals.setInputCloud (sphere_cloud);
segmentation_from_normals.setInputNormals (cloud_normals3);
// Obtain the sphere inliers and coefficients
segmentation_from_normals.segment (*inliers_sphere, *coefficients_sphere);
//std::cerr << "Sphere coefficients: " << *coefficients_sphere << std::endl;
// Publish the sphere cloud
extract_indices.setInputCloud (sphere_cloud);
extract_indices.setIndices (inliers_sphere);
extract_indices.setNegative (false);
extract_indices.filter (*sphere_output);
if (sphere_output->points.empty ())
std::cerr << "Can't find the sphere component." << std::endl;
pcl::toROSMsg (*sphere_output, *sphere_output_cloud);
sphere_pub.publish(sphere_output_cloud);
ros::Time sphere_end = ros::Time::now();
std::cout << "cloud size : " << cloud->width * cloud->height << std::endl;
std::cout << "plane size : " << transformed_cloud->width * transformed_cloud->height << std::endl;
//std::cout << "plane size : " << cloud_normals->width * cloud_normals->height << std::endl;
//std::cout << "cylinder size : " << cloud_normals2->width * cloud_normals2->height << std::endl;
std::cout << "sphere size : " << cloud_normals3->width * cloud_normals3->height << std::endl;
ros::Time whole_now = ros::Time::now();
printf("\n");
std::cout << "whole time : " << whole_now - whole_start << " sec" << std::endl;
std::cout << "declare types time : " << declare_types_end - declare_types_start << " sec" << std::endl;
//std::cout << "estimate time : " << estimate_end - estimate_start << " sec" << std::endl;
std::cout << "plane time : " << plane_end - plane_start << " sec" << std::endl;
std::cout << "rest and pass time : " << rest_pass_end - rest_pass_start << " sec" << std::endl;
std::cout << "sphere time : " << sphere_end - sphere_start << " sec" << std::endl;
printf("\n");
}
int main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_original(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr result_pass_filter0(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr result_pass_filter1(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr result_pass_filter2(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud2_planar(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud3_non_planar(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud4_cylinder(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud4_cylinder_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud5_non_planar_non_cylinder(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PCDReader reader;
//reader.read<pcl::PointXYZRGB> ("/home/niladri-64/plc_kinect_workspace/robot/build/two_objects/saved_file4.pcd",*cloud_original);
reader.read<pcl::PointXYZRGB> ("/home/niladri-64/plc_kinect_workspace/openni_grabber/build/saved_file2.pcd",*cloud_original);
// TRansforming
Eigen::Matrix4f temp_trans;
temp_trans << -0.819538 , -0.26171 , 0.50977, -0.316881 ,
-0.572858, 0.352719, -0.73988 ,0.944997 ,
0.013828, -0.898386, -0.438989 , 0.737386,
0, 0, 0, 1;
pcl::transformPointCloud(*cloud_original, *cloud, temp_trans );
{
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer = rgbVis(cloud);
while (!viewer->wasStopped())
{
viewer->spinOnce(1000);
}
}
pcl::PassThrough<pcl::PointXYZRGB> pass;
pass.setFilterFieldName ("z");
pass.setFilterLimits (-0.18, 0.5);
pass.setInputCloud (cloud);
pass.filter (*result_pass_filter0);
pass.setFilterFieldName ("x");
pass.setFilterLimits (0.77, 1.2);
pass.setInputCloud (result_pass_filter0);
pass.filter (*result_pass_filter1);
pass.setFilterFieldName ("y");
pass.setFilterLimits (-0.5, 0.5);
pass.setInputCloud (result_pass_filter1);
pass.filter (*result_pass_filter2);
{
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer = rgbVis(result_pass_filter2);
while (!viewer->wasStopped())
{
viewer->spinOnce(1000);
}
}
// Planar Segmentation
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.02);
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
int i = 0, nr_points = (int) result_pass_filter2->points.size ();
// While 30% of the original cloud is still there
while (result_pass_filter2->points.size () > 0.5 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (result_pass_filter2);
seg.segment (*inliers, *coefficients);
// std::cerr << "Model coefficients: " << coefficients->values[0] << " "
// << coefficients->values[1] << " "
// << coefficients->values[2] << " "
// << coefficients->values[3] << std::endl;
if (inliers->indices.size () == 0)
{
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the inliers
extract.setInputCloud (result_pass_filter2);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud2_planar);
std::cerr << "PointCloud representing the planar component: " << cloud2_planar->width * cloud2_planar->height << " data points." << std::endl;
extract.setNegative (true);
extract.filter (*cloud3_non_planar);
std::stringstream ss;
ss << "table_scene_lms400_plane_" << i << ".pcd";
//writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false);
{
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer = viewportsVis(cloud3_non_planar, result_pass_filter2);
while (!viewer->wasStopped())
{
viewer->spinOnce (1000);
}
}
i++;
result_pass_filter2.swap (cloud3_non_planar);
} // while loop
result_pass_filter2.swap (cloud3_non_planar);
// cloud3_non_planar is again the non-planar component
{ // cylinder segmentation
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals3 (new pcl::PointCloud<pcl::Normal>);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
// Estimate point normals
ne.setSearchMethod (tree);
ne.setInputCloud (cloud3_non_planar);
ne.setKSearch (50);
ne.compute (*cloud_normals3);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
// pcl::SACSegmentation<pcl::PointXYZRGB> seg2;
pcl::SACSegmentationFromNormals<pcl::PointXYZRGB, pcl::Normal> seg2;
// Optional
seg2.setOptimizeCoefficients (true);
seg2.setModelType (pcl::SACMODEL_CYLINDER);
seg2.setMethodType (pcl::SAC_RANSAC);
seg2.setNormalDistanceWeight (0.1);
seg2.setMaxIterations (10000);
seg2.setDistanceThreshold (0.07);
seg2.setRadiusLimits (0, 0.11);
seg2.setInputCloud (cloud3_non_planar);
seg2.setInputNormals (cloud_normals3);
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
int i = 0, nr_points = (int) cloud3_non_planar->points.size ();
// While 30% of the original cloud is still there
// while (result2->points.size () > 0.5 * nr_points)
// {
// Segment the largest planar component from the remaining cloud
seg2.setInputCloud (cloud3_non_planar);
seg2.segment (*inliers, *coefficients);
std::cerr << "Model coefficients of Cylinder : " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << " "
<< coefficients->values[4] << " "
<< coefficients->values[5] << " "
<< coefficients->values[6] << " " <<std::endl;
std::cout << "Orientation :" << coefficients->values[3] << " " << coefficients->values[4] << " "<< coefficients->values[5] << std::endl;
if (inliers->indices.size () == 0)
{
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
// break;
}
// Extract the inliers
extract.setInputCloud (cloud3_non_planar);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud4_cylinder);
std::cerr << "PointCloud representing the planar component: " << cloud4_cylinder->width * cloud4_cylinder->height << " data points." << std::endl;
std::stringstream ss;
ss << "table_scene_lms400_plane_" << i << ".pcd";
//writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false);
{ // visualizer
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer = viewportsVis(cloud4_cylinder, cloud3_non_planar);
while (!viewer->wasStopped())
{
viewer->spinOnce (1000);
}
}
// Create the filtering object
extract.setNegative (true);
extract.filter (*cloud5_non_planar_non_cylinder);
// cloud3_non_planar.swap (cloud5_non_planar_non_cylinder); we are not running a loop
//i++;
} // cylinder segmentation ()
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
sor.setInputCloud (cloud4_cylinder);
sor.setMeanK (50);
sor.setStddevMulThresh (1.0);
sor.filter (*cloud4_cylinder_filtered);
{
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer = viewportsVis(cloud4_cylinder_filtered, cloud4_cylinder);
while (!viewer->wasStopped())
{
viewer->spinOnce (1000);
}
}
pcl::PointXYZRGB minPt, maxPt;
pcl::getMinMax3D (*cloud4_cylinder_filtered, minPt, maxPt);
std::cout << "Max x: " << maxPt.x << std::endl;
std::cout << "Max y: " << maxPt.y << std::endl;
std::cout << "Max z: " << maxPt.z << std::endl;
std::cout << "Min x: " << minPt.x << std::endl;
std::cout << "Min y: " << minPt.y << std::endl;
std::cout << "Min z: " << minPt.z << std::endl;
std::cout << "Position :" << minPt.x << " " << (maxPt.y + minPt.y)/2 << " "<< (maxPt.z + minPt.z)/2 << std::endl;
//std::cout << "Orientation :" << coefficients->values[4] << " " << coefficients->values[5] << " "<< coefficients->values[6] << std::endl;
return 0;
}
