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;
}
int main(int argc, char** argv)
{
// initialize variables
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_original (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_unaltered (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_objects (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_planeInliers (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_planeOutliers (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients());
pcl::PointIndices::Ptr indices (new pcl::PointIndices());
pcl::PCDReader reader;
pcl::PCDWriter writer;
int result = 0; // catch function return
// enable variables for time logging
boost::posix_time::ptime time_before_execution;
boost::posix_time::ptime time_after_execution;
boost::posix_time::time_duration difference;
// read point cloud through command line
if (argc != 2)
{
std::cout << "usage: " << argv[0] << " <filename>\n";
return 0;
}
else
{ // read cloud and display its size
std::cout << "Reading Point Cloud" << std::endl;
reader.read(argv[1], *cloud_original);
#ifdef DEBUG
std::cout << "size of original cloud: "
<< cloud_original->points.size() << " points" << std::endl;
#endif
}
//**************************************************************************
// test passthroughFilter() function
#ifdef PASSTHROUGH_FILTER_TEST
std::cout << "RUNNING PASSTHROUGH FILTER TESTS" << std::endl;
log_passthroughFilter(cloud_original, cloud_filtered, "../pictures/T01_passthrough_01.pcd",
"passthrough filter, custom 1: ","z", -2.5, 0);
log_passthroughFilter(cloud_original, cloud_filtered, "../pictures/T01_passthrough_02.pcd",
"passthrough filter, custom 2: ","y", -3.0, 3.0);
log_passthroughFilter(cloud_original, cloud_filtered, "../pictures/T01_passthrough_03.pcd",
"passthrough filter, custom 3: ","x", -3.0, 3.0);
#endif
//**************************************************************************
// test voxelFilter() function
#ifdef VOXEL_FILTER_TEST
std::cout << "RUNNING VOXEL FILTER LEAF SIZE TEST" << std::endl;
test_voxelFilter_leafSize(cloud_original);
#endif
//**************************************************************************
// test removeNoise() function
#ifdef NOISE_REMOVAL_TEST
std::cout << "RUNNING NOISE REMOVAL NEAREST NEIGHBORS TEST" << std::endl;
test_removeNoise_neighbors(cloud_original);
std::cout << "RUNNING NOISE REMOVAL THRESHOLD TEST" << std::endl;
test_removeNoise_stdDeviation(cloud_original);
#endif
//**************************************************************************
// test getPrism() function
#ifdef EXTRACT_PRISM_DATA_TEST
std::cout << "RUNNING EXTRACT PRISM ITERATIONS TEST" << std::endl;
test_getPrism_iterations(cloud_original);
std::cout << "RUNNING EXTRACT PRISM THRESHOLD TEST" << std::endl;
test_getPrism_threshold(cloud_original);
#endif
//**************************************************************************
// test objectSegmentation() function
#ifdef SEGMENT_OBJECTS_TEST
std::cout << "RUNNING OBJECT SEGMENTATION TEST" << std::endl;
test_segmentObjects(cloud_original);
#endif
//**************************************************************************
// test objectSegmentation() function
#ifdef EUCLIDEAN_CLUSTER_TEST
std::cout << "RUNNING EUCLIDEAN CLUSTER TEST" << std::endl;
test_getClusters(cloud_original);
#endif
return 0;
}
int main(int argc, char** argv)
{
// initialize variables
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_original (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_unaltered (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_objects (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_planeInliers (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_planeOutliers (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients());
pcl::PointIndices::Ptr indices (new pcl::PointIndices());
pcl::PCDReader reader;
pcl::PCDWriter writer;
int result = 0; // catch function return
// enable variables for time logging
boost::posix_time::ptime time_before_execution;
boost::posix_time::ptime time_after_execution;
boost::posix_time::time_duration difference;
// read point cloud through command line
if (argc != 2)
{
std::cout << "usage: " << argv[0] << " <filename>\n";
return 0;
}
else
{ // read cloud and display its size
std::cout << "Reading Point Cloud" << std::endl;
reader.read(argv[1], *cloud_original);
#ifdef DEBUG
std::cout << "size of original cloud: "
<< cloud_original->points.size() << " points" << std::endl;
#endif
}
//**************************************************************************
// test passthroughFilter() function
std::cout << "RUNNING PASSTHROUGH FILTER TESTS" << std::endl;
log_passthroughFilter(cloud_original, cloud_filtered, "../pictures/T01_passthrough_01.pcd",
"passthrough filter, custom 1: ","z", -2.5, 0);
log_passthroughFilter(cloud_original, cloud_filtered, "../pictures/T01_passthrough_02.pcd",
"passthrough filter, custom 2: ","y", -3.0, 3.0);
log_passthroughFilter(cloud_original, cloud_filtered, "../pictures/T01_passthrough_03.pcd",
"passthrough filter, custom 3: ","x", -3.0, 3.0);
//**************************************************************************
// test voxelFilter() function
std::cout << "RUNNING VOXEL FILTER TEST" << std::endl;
log_voxelFilter(cloud_original, cloud_filtered, "../pictures/T02_voxel_01.pcd",
"voxel filter, custom 1:", 0.01);
log_voxelFilter(cloud_original, cloud_filtered, "../pictures/T02_voxel_02.pcd",
"voxel filter, custom 2:", 0.02);
log_voxelFilter(cloud_original, cloud_filtered, "../pictures/T02_voxel_03.pcd",
"voxel filter, custom 3:", 0.04);
log_voxelFilter(cloud_original, cloud_filtered, "../pictures/T02_voxel_04.pcd",
"voxel filter, custom 4:", 0.08);
//**************************************************************************
// test removeNoise() function
std::cout << "RUNNING NOISE REMOVAL TEST" << std::endl;
log_removeNoise(cloud_original, cloud_filtered, "../pictures/T03_noise_01.pcd",
"noise removal, custom 1: ", 50, 1.0);
log_removeNoise(cloud_original, cloud_filtered, "../pictures/T03_noise_01.pcd",
"noise removal, custom 1: ", 100, 1.0);
log_removeNoise(cloud_original, cloud_filtered, "../pictures/T03_noise_01.pcd",
"noise removal, custom 1: ", 10, 1.0);
log_removeNoise(cloud_original, cloud_filtered, "../pictures/T03_noise_01.pcd",
"noise removal, custom 1: ", 50, 1.9);
log_removeNoise(cloud_original, cloud_filtered, "../pictures/T03_noise_01.pcd",
"noise removal, custom 1: ", 50, 0.1);
//**************************************************************************
// test getPlane() function
std::cout << "RUNNING PLANE SEGMENTATION TEST" << std::endl;
log_getPlane(cloud_original, cloud_planeInliers, cloud_planeOutliers,
coefficients, indices, "../pictures/T04_planeInliers_01.pcd",
"plane segmentation, custom 1: ", 0, 1.57, 1000, 0.01);
log_getPlane(cloud_original, cloud_planeInliers, cloud_planeOutliers,
coefficients, indices, "../pictures/T04_planeInliers_02.pcd",
"plane segmentation, custom 2: ", 0, 1.57, 1, 0.01);
log_getPlane(cloud_original, cloud_planeInliers, cloud_planeOutliers,
coefficients, indices, "../pictures/T04_planeInliers_03.pcd",
"plane segmentation, custom 3: ", 0, 1.57, 2000, 0.01);
log_getPlane(cloud_original, cloud_planeInliers, cloud_planeOutliers,
coefficients, indices, "../pictures/T04_planeInliers_04.pcd",
"plane segmentation, custom 4: ", 0, 0.76, 1000, 0.01);
log_getPlane(cloud_original, cloud_planeInliers, cloud_planeOutliers,
coefficients, indices, "../pictures/T04_planeInliers_05.pcd",
"plane segmentation, custom 5: ", 0, 2.09, 1000, 0.01);
log_getPlane(cloud_original, cloud_planeInliers, cloud_planeOutliers,
coefficients, indices, "../pictures/T04_planeInliers_06.pcd",
"plane segmentation, custom 6: ", 0.35, 1.57, 1000, 0.01);
log_getPlane(cloud_original, cloud_planeInliers, cloud_planeOutliers,
coefficients, indices, "../pictures/T04_planeInliers_07.pcd",
"plane segmentation, custom 7: ", 0.79, 1.57, 1000, 0.01);
log_getPlane(cloud_original, cloud_planeInliers, cloud_planeOutliers,
coefficients, indices, "../pictures/T04_planeInliers_08.pcd",
"plane segmentation, custom 8: ", 0, 1.57, 1000, 0.001);
log_getPlane(cloud_original, cloud_planeInliers, cloud_planeOutliers,
coefficients, indices, "../pictures/T04_planeInliers_09.pcd",
"plane segmentation, custom 9: ", 0, 1.57, 1000, 0.01);
//**************************************************************************
// test getPrism() function
std::cout << "RUNNING EXTRACT PRISM TEST" << std::endl;
log_getPrism(cloud_original, cloud_objects, "../pictures/T05_objects_01.pcd",
"polygonal prism, custom 1: ", 0, 1.57, 1000, 0.01, 0.02, 0.2);
// test getPrism() function
std::cout << "RUNNING EXTRACT PRISM TEST" << std::endl;
// get output from default plane
time_before_execution = boost::posix_time::microsec_clock::local_time(); // time before
result = c44::getPrism(cloud_original, cloud_objects, 0, 1.57, 1000, 0.01, 0.02, 0.2);
time_after_execution = boost::posix_time::microsec_clock::local_time(); // time after
if(result < 0)
{
std::cout << "ERROR: could not find polygonal prism data" << std::endl;
}
else
{
writer.write<pcl::PointXYZ> ("../pictures/T05_objects_01.pcd", *cloud_objects); // write out cloud
#ifdef DEBUG
difference = time_after_execution - time_before_execution; // get execution time
std::cout << std::setw(5) << difference.total_milliseconds() << ": "
<< "polygonal prism, default: "
<< cloud_objects->points.size() << " points" << std::endl;
#endif
}
return 0;
}
