int
main(int argc, char** argv)
{
// initialize PointClouds
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);
// populate our PointCloud with points
cloud->width = 500;
cloud->height = 1;
cloud->is_dense = false;
cloud->points.resize (cloud->width * cloud->height);
for (size_t i = 0; i < cloud->points.size (); ++i)
{
if (pcl::console::find_argument (argc, argv, "-s") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)
{
cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
if (i % 5 == 0)
cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0);
else if(i % 2 == 0)
cloud->points[i].z = sqrt( 1 - (cloud->points[i].x * cloud->points[i].x)
- (cloud->points[i].y * cloud->points[i].y));
else
cloud->points[i].z = - sqrt( 1 - (cloud->points[i].x * cloud->points[i].x)
- (cloud->points[i].y * cloud->points[i].y));
}
else
{
cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
if( i % 2 == 0)
cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0);
else
cloud->points[i].z = -1 * (cloud->points[i].x + cloud->points[i].y);
}
}
std::vector<int> inliers;
// created RandomSampleConsensus object and compute the appropriated model
pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr
model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (cloud));
pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud));
if(pcl::console::find_argument (argc, argv, "-f") >= 0)
{
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_p);
ransac.setDistanceThreshold (.01);
ransac.computeModel();
ransac.getInliers(inliers);
}
else if (pcl::console::find_argument (argc, argv, "-sf") >= 0 )
{
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_s);
ransac.setDistanceThreshold (.01);
ransac.computeModel();
ransac.getInliers(inliers);
}
// copies all inliers of the model computed to another PointCloud
pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);
// creates the visualization object and adds either our orignial cloud or all of the inliers
// depending on the command line arguments specified.
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
if (pcl::console::find_argument (argc, argv, "-f") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)
viewer = simpleVis(final);
else
int main(int argc, char** argv) {
//check if number of arguments is proper
if(argc!=3){
help();
exit(0);
}
// initialize PointClouds
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) == -1) //* load the file
{
cerr << "Couldn't read file :" << argv[1] << "\n";
return (-1);
}
std::vector<int> inliers;
// created RandomSampleConsensus object and compute the appropriated model
pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ>(cloud));
pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));
//RANSAC for Line model
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line(new pcl::PointCloud<PointT>());
if (pcl::console::find_argument(argc, argv, "-l") >= 0) {
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers_l(new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients(true);
// Mandatory
seg.setModelType(pcl::SACMODEL_LINE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.05);
seg.setInputCloud(cloud);
seg.segment(*inliers_l, *coefficients);
// Write the line inliers to disk
pcl::ExtractIndices<PointT> extract;
extract.setInputCloud(cloud);
extract.setIndices(inliers_l);
extract.setNegative(false);
extract.filter(*cloud_line);
}
if (pcl::console::find_argument(argc, argv, "-f") >= 0) {
//RANSAC for Plane model
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);
ransac.setDistanceThreshold(.01);
ransac.computeModel();
ransac.getInliers(inliers);
} else if (pcl::console::find_argument(argc, argv, "-sf") >= 0) {
//RANSAC for Sphere model
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_s);
ransac.setDistanceThreshold(.01);
ransac.computeModel();
ransac.getInliers(inliers);
}
// copies all inliers of the model computed to another PointCloud
pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);
// creates the visualization object and adds either our orignial cloud or all of the inliers
// depending on the command line arguments specified.
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
if (pcl::console::find_argument(argc, argv, "-f") >= 0 || pcl::console::find_argument(argc, argv, "-sf") >= 0) {
viewer = simpleVis(final);
} else if (pcl::console::find_argument(argc, argv, "-l") >= 0) {
void callback(const sensor_msgs::PointCloud2::ConstPtr& 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;
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
std::vector<int> inliers;
// The plane and sphere coefficients
pcl::ModelCoefficients::Ptr coefficients_sphere (new pcl::ModelCoefficients ());
// The plane and sphere inliers
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 passthrough_filtered (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr sphere_output_cloud (new sensor_msgs::PointCloud2);
// The PointCloud
pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_output (new pcl::PointCloud<pcl::PointXYZ>);
ros::Time declare_types_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Create Voxel grid 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);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Pass through Filtering
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time pass_start = ros::Time::now();
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 3.5);
pass.filter (*passthrough_filtered);
pass.setInputCloud (passthrough_filtered);
pass.setFilterFieldName ("y");
pass.setFilterLimits (-0.1, 0.3);
pass.filter (*passthrough_filtered);
passthrough_pub.publish(passthrough_filtered);
ros::Time pass_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* for sphere features
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromROSMsg (*passthrough_filtered, *sphere_cloud);
ros::Time sphere_start = ros::Time::now();
// created RandomSampleConsensus object and compute the appropriated model
pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (sphere_cloud));
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_s);
ransac.setDistanceThreshold (.01);
ransac.computeModel();
ransac.getInliers(inliers);
// copies all inliers of the model computed to another PointCloud
pcl::copyPointCloud<pcl::PointXYZ>(*sphere_cloud, inliers, *sphere_output);
pcl::toROSMsg (*sphere_output, *sphere_output_cloud);
sphere_pub.publish(sphere_output_cloud);
// // Optional
// seg.setOptimizeCoefficients (true);
// // Mandatory
// seg.setModelType (pcl::SACMODEL_SPHERE);
// seg.setMethodType (pcl::SAC_RANSAC);
// seg.setMaxIterations (10000);
// seg.setDistanceThreshold (0.05);
// seg.setRadiusLimits (0, 0.15);
// seg.setInputCloud (sphere_cloud);
// seg.segment (*inliers_sphere, *coefficients_sphere);
// //std::cerr << "Sphere coefficients: " << *coefficients_sphere << std::endl;
//
//
// 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();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* visualize normals
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// pcl::visualization::PCLVisualizer viewer("PCL Viewer");
// viewer.setBackgroundColor (0.0, 0.0, 0.5);
// viewer.addPointCloudNormals<pcl::PointXYZ,pcl::Normal>(sphere_cloud, cloud_normals3);
// viewer.spin();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time whole_end = ros::Time::now();
std::cout << "cloud size : " << cloud->width * cloud->height << std::endl;
std::cout << "pass_th size : " << passthrough_filtered->width * passthrough_filtered->height << std::endl;
std::cout << "sphere size : " << sphere_output_cloud->width * sphere_output_cloud->height << std::endl;
printf("\n");
std::cout << "whole time : " << whole_end - whole_start << " sec" << std::endl;
std::cout << "declare types time : " << declare_types_end - declare_types_start << " sec" << std::endl;
std::cout << "passthrough time : " << pass_end - pass_start << " sec" << std::endl;
std::cout << "sphere time : " << sphere_end - sphere_start << " sec" << std::endl;
printf("\n----------------------------------------------------------------------------\n");
printf("\n");
}
void callback(const sensor_msgs::PointCloud2::ConstPtr& 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::PointXYZ> extract_indices2;
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// The plane and sphere coefficients
pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients ());
pcl::ModelCoefficients::Ptr coefficients_sphere (new pcl::ModelCoefficients ());
// The plane and sphere inliers
pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices ());
pcl::PointIndices::Ptr inliers_sphere (new pcl::PointIndices ());
// The point clouds
sensor_msgs::PointCloud2::Ptr passthrough_filtered (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr plane_seg_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr sphere_RANSAC_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr rest_whole_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr rest_cloud_filtered (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr sphere_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr whole_pc (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr ball_candidate_output_cloud (new sensor_msgs::PointCloud2);
// The PointCloud
pcl::PointCloud<pcl::PointXYZ>::Ptr plane_seg_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr plane_seg_output (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr remove_plane_cloud (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>);
pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_RANSAC_output (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr remove_false_ball_candidate (new pcl::PointCloud<pcl::PointXYZ>);
std::vector<int> inliers;
ros::Time declare_types_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Pass through Filtering
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time pass_start = ros::Time::now();
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 2.5);
pass.filter (*passthrough_filtered);
passthrough_pub.publish(passthrough_filtered);
ros::Time pass_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* for plane features pcl::SACSegmentation
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time plane_seg_start = ros::Time::now();
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromROSMsg (*passthrough_filtered, *plane_seg_cloud);
// Optional
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setAxis(Eigen::Vector3f (0, -1, 0)); // best plane should be perpendicular to z-axis
seg.setMaxIterations (40);
seg.setDistanceThreshold (0.05);
seg.setInputCloud (plane_seg_cloud);
seg.segment (*inliers_plane, *coefficients_plane);
extract_indices.setInputCloud(plane_seg_cloud);
extract_indices.setIndices(inliers_plane);
extract_indices.setNegative(false);
extract_indices.filter(*plane_seg_output);
pcl::toROSMsg (*plane_seg_output, *plane_seg_output_cloud);
plane_pub.publish(plane_seg_output_cloud);
ros::Time plane_seg_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_plane_cloud);
plane_seg_cloud.swap (remove_plane_cloud);
// publish result of Removal the planar inliers, extract the rest
pcl::toROSMsg (*plane_seg_cloud, *rest_whole_cloud);
rest_whole_pub.publish(rest_whole_cloud); // 'rest_whole_cloud' substituted whole_pc
ros::Time rest_pass_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time find_ball_start = ros::Time::now();
int iter = 0;
while(iter < 5)
{
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* for sphere features pcl::SACSegmentation
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromROSMsg (*rest_whole_cloud, *sphere_cloud);
ros::Time sphere_start = ros::Time::now();
// Optional
seg.setOptimizeCoefficients (false);
// Mandatory
seg.setModelType (pcl::SACMODEL_SPHERE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (10000);
seg.setDistanceThreshold (0.03);
seg.setRadiusLimits (0.12, 0.16);
seg.setInputCloud (sphere_cloud);
seg.segment (*inliers_sphere, *coefficients_sphere);
//std::cerr << "Sphere coefficients: " << *coefficients_sphere << std::endl;
if (inliers_sphere->indices.empty())
std::cerr << "[[--Can't find the Sphere Candidate.--]]" << std::endl;
else {
extract_indices.setInputCloud(sphere_cloud);
extract_indices.setIndices(inliers_sphere);
extract_indices.setNegative(false);
extract_indices.filter(*sphere_output);
pcl::toROSMsg (*sphere_output, *sphere_output_cloud);
sphere_seg_pub.publish(sphere_output_cloud); // 'sphere_output_cloud' means ball candidate point cloud
}
ros::Time sphere_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* for sphere features pcl::SampleConsensusModelSphere
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time sphere_RANSAC_start = ros::Time::now();
// created RandomSampleConsensus object and compute the appropriated model
pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (sphere_output));
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_s);
ransac.setDistanceThreshold (.01);
ransac.computeModel();
ransac.getInliers(inliers);
// copies all inliers of the model computed to another PointCloud
pcl::copyPointCloud<pcl::PointXYZ>(*sphere_output, inliers, *sphere_RANSAC_output);
pcl::toROSMsg (*sphere_RANSAC_output, *sphere_RANSAC_output_cloud);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* To discriminate a ball
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
double w = 0;
w = double(sphere_RANSAC_output_cloud->width * sphere_RANSAC_output_cloud->height)
/double(sphere_output_cloud->width * sphere_output_cloud->height);
if (w > 0.9) {
BALL = true;
std::cout << "can find a ball" << std::endl;
true_ball_pub.publish(sphere_RANSAC_output_cloud);
break;
} else {
BALL = false;
std::cout << "can not find a ball" << std::endl;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Exclude false ball candidate
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//ros::Time rest_pass_start = ros::Time::now();
// Create the filtering object
// Remove the planar inliers, extract the rest
extract_indices2.setInputCloud(plane_seg_cloud);
extract_indices2.setIndices(inliers_sphere);
extract_indices2.setNegative (true);
extract_indices2.filter (*remove_false_ball_candidate);
sphere_RANSAC_output.swap (remove_false_ball_candidate);
// publish result of Removal the planar inliers, extract the rest
pcl::toROSMsg (*sphere_RANSAC_output, *ball_candidate_output_cloud);
rest_ball_candidate_pub.publish(ball_candidate_output_cloud);
rest_whole_cloud = ball_candidate_output_cloud;
}
iter++;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time sphere_RANSAC_end = ros::Time::now();
}
ros::Time find_ball_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time whole_end = ros::Time::now();
std::cout << "cloud size : " << cloud->width * cloud->height << std::endl;
std::cout << "plane size : " << plane_seg_output_cloud->width * plane_seg_output_cloud->height << std::endl;
std::cout << "rest size : " << rest_whole_cloud->width * rest_whole_cloud->height << std::endl;
std::cout << "sphere size : " << sphere_output_cloud->width * sphere_output_cloud->height << std::endl;
std::cout << "sphere RANSAC size : " << sphere_RANSAC_output_cloud->width * sphere_RANSAC_output_cloud->height << " " << sphere_RANSAC_output->points.size() << std::endl;
std::cout << "sphereness : " << double(sphere_RANSAC_output_cloud->width * sphere_RANSAC_output_cloud->height)
/double(sphere_output_cloud->width * sphere_output_cloud->height) << std::endl;
std::cout << "model coefficient : " << coefficients_plane->values[0] << " "
<< coefficients_plane->values[1] << " "
<< coefficients_plane->values[2] << " "
<< coefficients_plane->values[3] << " " << std::endl;
printf("\n");
std::cout << "whole time : " << whole_end - whole_start << " sec" << std::endl;
std::cout << "declare types time : " << declare_types_end - declare_types_start << " sec" << std::endl;
std::cout << "passthrough time : " << pass_end - pass_start << " sec" << std::endl;
std::cout << "plane time : " << plane_seg_end - plane_seg_start << " sec" << std::endl;
std::cout << "rest and pass time : " << rest_pass_end - rest_pass_start << " sec" << std::endl;
std::cout << "find a ball time : " << find_ball_end - find_ball_start << " sec" << std::endl;
//std::cout << "sphere time : " << sphere_end - sphere_start << " sec" << std::endl;
//std::cout << "sphere ransac time : " << sphere_RANSAC_end - sphere_RANSAC_start << " sec" << std::endl;
printf("\n----------------------------------------------------------------------------\n");
printf("\n");
}
