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;
}
void PclExtractor::cloudCallback(const Cloud2Msg::ConstPtr& cloud2Msg_input)
{
boost::mutex::scoped_lock(mutex_);
sensor_msgs::PointCloud2 output;
sensor_msgs::PointCloud2 convex_hull;
sensor_msgs::PointCloud2 object_msg;
sensor_msgs::PointCloud2 nonObject_msg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg (*cloud2Msg_input, *cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// *** Normal estimation
// Create the normal estimation class and pass the input dataset to it
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud(cloud);
// Creating the kdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
ne.setSearchMethod(tree);
// output dataset
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
// use all neighbors in a sphere of radius 3cm
ne.setRadiusSearch(0.3);
// compute the features
ne.compute(*cloud_normals);
// *** End of normal estimation
// *** Plane Estimation From Normals Start
// Create the segmentation object
pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
// Optional
seg.setOptimizeCoefficients(true);
// Mandatory
// seg.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
seg.setModelType(pcl::SACMODEL_PLANE);
// seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
//y가 z
// const Eigen::Vector3f z_axis(0,-1.0,0);
// seg.setAxis(z_axis);
// seg.setEpsAngle(seg_setEpsAngle_);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations (seg_setMaxIterations_);
seg.setDistanceThreshold(seg_setDistanceThreshold_);
seg.setNormalDistanceWeight(seg_setNormalDistanceWeight_);
// seg.setProbability(seg_probability_);
seg.setInputCloud((*cloud).makeShared());
seg.setInputNormals(cloud_normals);
seg.segment(*inliers, *coefficients);
std::cerr <<"input: "<<cloud->width*cloud->height<<"Model inliers: " << inliers->indices.size () << std::endl;
if (inliers->indices.size () == 0)
{
ROS_ERROR ("Could not estimate a planar model for the given dataset.");
}
// *** End of Plane Estimation
// *** Plane Estimation Start
// Create the segmentation object
/* pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
//seg.setOptimizeCoefficients(true);
// Mandatory
seg.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
// seg.setModelType(pcl::SACMODEL_PLANE);
// seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
//y가 z
const Eigen::Vector3f z_axis(0,-1.0,0);
seg.setAxis(z_axis);
seg.setEpsAngle(seg_setEpsAngle_);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations (seg_setMaxIterations_);
seg.setDistanceThreshold(seg_setDistanceThreshold_);
seg.setInputCloud((*cloud).makeShared());
seg.segment(*inliers, *coefficients);
std::cerr <<"input: "<<cloud->width*cloud->height<<"Model inliers: " << inliers->indices.size () << std::endl;
if (inliers->indices.size () == 0)
{
ROS_ERROR ("Could not estimate a planar model for the given dataset.");
}
// *** End of Plane Estimation
*/
pcl::ExtractIndices<pcl::PointXYZ> extract;
// Extrat the inliers
extract.setInputCloud(cloud);
extract.setIndices(inliers);
pcl::PointCloud<pcl::PointXYZ>::Ptr ground_hull(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>);
if ((int)inliers->indices.size() > minPoints_)
{
extract.setNegative(false);
extract.filter(*cloud_p);
pcl::toROSMsg(*cloud_p, output);
std::cerr << "PointCloud representing the planar component: "
<< cloud_p->width * cloud_p->height << " data points." << std::endl;
// Step 3c. Project the ground inliers
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType(pcl::SACMODEL_PLANE);
proj.setInputCloud(cloud_p);
proj.setModelCoefficients(coefficients);
proj.filter(*cloud_projected);
// Step 3d. Create a Convex Hull representation of the projected inliers
pcl::ConvexHull<pcl::PointXYZ> chull;
//chull.setInputCloud(cloud_p);
chull.setInputCloud(cloud_projected);
chull.setDimension(chull_setDimension_);//2D
chull.reconstruct(*ground_hull);
pcl::toROSMsg(*ground_hull, convex_hull);
//pcl::toROSMsg(*ground_hull, convex_hull);
ROS_INFO ("Convex hull has: %d data points.", (int) ground_hull->points.size ());
// Publish the data
plane_pub_.publish (output);
hull_pub_.publish(convex_hull);
}
// Create the filtering object
extract.setNegative(true);
extract.filter(*cloud_f);
ROS_INFO ("Cloud_f has: %d data points.", (int) cloud_f->points.size ());
// cloud.swap(cloud_f);
pcl::PointCloud<pcl::PointXYZ>::Ptr object(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr nonObject(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointIndices::Ptr object_indices(new pcl::PointIndices);
// cloud statictical filter
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudStatisticalFiltered (new pcl::PointCloud<pcl::PointXYZ>);
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud(cloud_f);
sor.setMeanK(sor_setMeanK_);
sor.setStddevMulThresh(sor_setStddevMulThresh_);
sor.filter(*cloudStatisticalFiltered);
pcl::ExtractIndices<pcl::PointXYZ> exObjectIndices;
//exObjectIndices.setInputCloud(cloud_f);
exObjectIndices.setInputCloud(cloudStatisticalFiltered);
exObjectIndices.setIndices(object_indices);
exObjectIndices.filter(*object);
exObjectIndices.setNegative(true);
exObjectIndices.filter(*nonObject);
ROS_INFO ("Object has: %d data points.", (int) object->points.size ());
pcl::toROSMsg(*object, object_msg);
object_pub_.publish(object_msg);
//pcl::toROSMsg(*nonObject, nonObject_msg);
//pcl::toROSMsg(*cloud_f, nonObject_msg);
pcl::toROSMsg(*cloudStatisticalFiltered, nonObject_msg);
nonObject_pub_.publish(nonObject_msg);
}
int
main (int argc, char** argv)
{
// Read in the cloud data
pcl::PCDReader reader;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
reader.read ("../bottle.pcd", *cloud);
std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //*
parseCommandLine(argc, argv);
std::cout << "argc:" << argc << " argv:" << *argv << std::endl;
std::cout << "x_min:" << x_pass_min_ << " x_max:" << x_pass_max_ << " y_min:" << y_pass_min_ << " y_max:"<<y_pass_max_<<std::endl;
/*apply pass through filter*/
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
// Create the filtering object
pcl::PassThrough<pcl::PointXYZ> pass;
//pass.setFilterLimitsNegative (true);
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (z_pass_min_, z_pass_max_);
pass.filter (*cloud_filtered);
pass.setInputCloud (cloud_filtered);
pass.setFilterFieldName ("y");
pass.setFilterLimits (y_pass_min_, y_pass_max_);
pass.filter (*cloud_filtered);
pass.setInputCloud (cloud_filtered);
pass.setFilterFieldName ("x");
pass.setFilterLimits (x_pass_min_, x_pass_max_);
pass.filter (*cloud_filtered);
viewer.addPointCloud<PointT> (cloud_filtered, "input_cloud");
#if 0
// while (!viewer.wasStopped ())
{
// viewer.spinOnce ();
}
return(0);
#endif
viewer.removeAllPointClouds();
cloud = cloud_filtered;
#if 0
#if 0
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PointXYZ> vg;
//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
vg.setInputCloud (cloud);
vg.setLeafSize (0.01f, 0.01f, 0.01f);
vg.filter (*cloud_filtered);
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //*
#endif
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PCDWriter writer;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
int i=0, nr_points = (int) cloud_filtered->points.size ();
while (cloud_filtered->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
// Get the points associated with the planar surface
extract.filter (*cloud_plane);
std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
*cloud_filtered = *cloud_f;
}
viewer.addPointCloud<PointT> (cloud_filtered, "input_cloud");
while (!viewer.wasStopped ())
{
viewer.spinOnce ();
}
#endif
#if 1
pcl::PCDWriter writer;
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud_filtered);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
cout << "ss:" << j << std::endl;
std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
std::stringstream ss;
ss << "cloud_cluster_" << j << ".pcd";
cout << "ss:" << j << std::endl;
writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*
j++;
}
return (0);
#endif
}
/** Set the number of coefficients in m_numCoef
* based on the current degree held in m_d.
*/
void Algebraic::calcNumCoef()
{
m_numCoef = coefficients(m_d);
}
void ObjectDetector::performSegmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ>), cloud_plane_rotated (new pcl::PointCloud<pcl::PointXYZ>);
ROS_INFO("PointCloud before planar segmentation: %d data points.", cloud->width * cloud->height);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Fit plane
seg.setModelType (pcl::SACMODEL_PLANE);
// Use RANSAC
seg.setMethodType (pcl::SAC_RANSAC);
// Set maximum number of iterations
seg.setMaxIterations (max_it_calibration);
// Set distance to the model threshold
seg.setDistanceThreshold (floor_threshold);
// Segment the largest planar component from the cloud
seg.setInputCloud (cloud);
seg.segment (*inliers, *coefficients);
// Extract the inliers of the plane
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_plane);
ROS_INFO("PointCloud representing the planar component: %d data points.", cloud_plane->width * cloud_plane->height);
// Create normal vector of the plane
Eigen::Matrix<float, 1, 3> normal_plane, normal_floor, r_axis;
normal_plane[0] = coefficients->values[0];
normal_plane[1] = coefficients->values[1];
normal_plane[2] = coefficients->values[2];
ROS_INFO("Plane normal: %f %f %f", normal_plane[0], normal_plane[1], normal_plane[2]);
// Create normal vector of the floor
normal_floor[0] = 0.0f;
normal_floor[1] = 1.0f;
normal_floor[2] = 0.0f;
ROS_INFO("Floor normal: %f %f %f", normal_floor[0], normal_floor[1], normal_floor[2]);
// Determine rotation axis
r_axis = normal_plane.cross(normal_floor);
ROS_INFO("Rotation axis: %f %f %f", r_axis[0], r_axis[1], r_axis[2]);
// Determine rotation angle theta
theta = acos(normal_plane.dot(normal_floor));
ROS_INFO("Rotation angle theta: %f", theta);
// Create rotation matrix
Eigen::Affine3f transform = Eigen::Affine3f::Identity();
transform.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));
// Perform rotation on extracted plane
pcl::transformPointCloud(*cloud_plane, *cloud_plane_rotated,transform);
// Compute y translation by taking the average y values of the plane points
int num_of_points = cloud_plane_rotated->width * cloud_plane_rotated->height;
for (size_t i = 0; i < num_of_points; ++i)
{
y_translation += cloud_plane_rotated->points[i].y;
}
y_translation = y_translation / num_of_points;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr ObjectDetector::filterCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
ROS_INFO("Before passthrough: %d", cloud->width * cloud->height);
if((cloud->width * cloud->height) != 0){
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered_x(new pcl::PointCloud<pcl::PointXYZ>), cloud_filtered_y (new pcl::PointCloud<pcl::PointXYZ>), cloud_filtered_z (new pcl::PointCloud<pcl::PointXYZ>);
// Filter x outliers
pcl::PassThrough<pcl::PointXYZ> pass_x;
pass_x.setInputCloud (cloud);
pass_x.setFilterFieldName ("x");
pass_x.setFilterLimits (-x_max, x_max);
pass_x.filter (*cloud_filtered_x);
// Filter y outliers
pcl::PassThrough<pcl::PointXYZ> pass_y;
pass_y.setInputCloud (cloud_filtered_x);
pass_y.setFilterFieldName ("y");
pass_y.setFilterLimits (floor_threshold, y_max);
pass_y.filter (*cloud_filtered_y);
// Filter z outliers
pcl::PassThrough<pcl::PointXYZ> pass_z;
pass_z.setInputCloud (cloud_filtered_y);
pass_z.setFilterFieldName ("z");
pass_z.setFilterLimits (-z_max, 0.0);
pass_z.filter (*cloud_filtered_z);
ROS_INFO("Before RANSAC: %d", cloud_filtered_z->width * cloud_filtered_z->height);
if((cloud_filtered_z->width * cloud_filtered_z->height) > min_points_left)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ransac (new pcl::PointCloud<pcl::PointXYZ>);
cloud_ransac = cloud_filtered_z;
pcl::ModelCoefficients::Ptr ransac_coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Fit plane
seg.setModelType (pcl::SACMODEL_PLANE);
// Use RANSAC
seg.setMethodType (pcl::SAC_RANSAC);
// Set maximum number of iterations
seg.setMaxIterations (max_it_runtime);
// Set distance to the model threshold
seg.setDistanceThreshold (wall_threshold);
// Segment the largest planar component from the cloud
seg.setInputCloud (cloud_ransac);
seg.segment (*inliers, *ransac_coefficients);
while(inliers->indices.size() > ransac_removal_threshold){
// Extract the inliers
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud_ransac);
extract.setIndices (inliers);
extract.setNegative (true);
extract.filter (*cloud_ransac);
pcl::SACSegmentation<pcl::PointXYZ> seg2;
// Optional
seg2.setOptimizeCoefficients (true);
// Fit plane
seg2.setModelType (pcl::SACMODEL_PLANE);
// Use RANSAC
seg2.setMethodType (pcl::SAC_RANSAC);
// Set maximum number of iterations
seg2.setMaxIterations (max_it_runtime);
// Set distance to the model threshold
seg2.setDistanceThreshold (wall_threshold);
// Segment the largest planar component from the cloud
seg2.setInputCloud (cloud_ransac);
seg2.segment (*inliers, *ransac_coefficients);
}
ROS_INFO("After RANSAC: %d", cloud_ransac->width * cloud_ransac->height);
if((cloud_ransac->width * cloud_ransac->height) > min_points_left){
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered_inc_walls (new pcl::PointCloud<pcl::PointXYZ>), cloud_filtered_only_walls (new pcl::PointCloud<pcl::PointXYZ>), cloud_projected_iw (new pcl::PointCloud<pcl::PointXYZ>), cloud_projected_ow (new pcl::PointCloud<pcl::PointXYZ>), cloud_projected_filtered (new pcl::PointCloud<pcl::PointXYZ>);
// Points containing both walls + objects
pcl::PassThrough<pcl::PointXYZ> pass_iw;
pass_iw.setInputCloud (cloud_ransac);
pass_iw.setFilterFieldName ("y");
pass_iw.setFilterLimits (floor_threshold, object_wall_threshold);
pass_iw.filter (*cloud_filtered_inc_walls);
// Points only containing walls
pcl::PassThrough<pcl::PointXYZ> pass_ow;
pass_ow.setInputCloud (cloud_ransac);
pass_ow.setFilterFieldName ("y");
pass_ow.setFilterLimits (object_wall_threshold, y_max);
pass_ow.filter (*cloud_filtered_only_walls);
if(((cloud_filtered_inc_walls->width * cloud_filtered_inc_walls->height) != 0) && ((cloud_filtered_only_walls->width * cloud_filtered_only_walls->height) != 0) )
{
// Create a set of planar coefficients with X=Z=0,Y=1
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
coefficients->values.resize (4);
coefficients->values[0] = coefficients->values[2] = 0;
coefficients->values[1] = 1.0;
coefficients->values[3] = 0;
// Eliminate y-axis by projection to 2D
pcl::ProjectInliers<pcl::PointXYZ> proj1;
proj1.setModelType (pcl::SACMODEL_PLANE);
proj1.setInputCloud (cloud_filtered_inc_walls);
proj1.setModelCoefficients (coefficients);
proj1.filter (*cloud_projected_iw);
pcl::ProjectInliers<pcl::PointXYZ> proj2;
proj2.setModelType (pcl::SACMODEL_PLANE);
proj2.setInputCloud (cloud_filtered_only_walls);
proj2.setModelCoefficients (coefficients);
proj2.filter (*cloud_projected_ow);
// Remove projected walls from cloud_projected_iw
if((cloud_projected_iw->width * cloud_projected_iw->height) != 0)
{
// Create kdtree
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud (cloud_projected_iw);
double radius = wall_threshold;
pcl::PointIndices::Ptr wallpoint_indices(new pcl::PointIndices);
// Create a set of already included indices
boost::unordered::unordered_set<int> inc_indices;
// Iterate over all points in cloud containing only walls
for (int i = 0; i < cloud_projected_ow->size(); i++)
{
pcl::PointXYZ owPoint = cloud_projected_ow->at(i) ;
std::vector<int> k_indices;
std::vector<float> k_distances;
// Find points which are closes neighbors to the walls
if(kdtree.radiusSearch (owPoint, radius, k_indices, k_distances) > 0){
// Iterate over points
for(std::vector<int>::size_type i = 0; i != k_indices.size(); i++) {
int index = k_indices[i];
// If no match is found for index, add to set & wallpoint indices
if (inc_indices.find(index) == inc_indices.end()){
inc_indices.insert(index);
wallpoint_indices->indices.push_back(index);
}
}
}
}
if(wallpoint_indices->indices.size() > 0){
ROS_INFO("Filter points: %d", int(wallpoint_indices->indices.size()));
// Remove the wall points
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud_filtered_inc_walls);
extract.setIndices (wallpoint_indices);
extract.setNegative (true);
extract.filter (*cloud_projected_filtered);
}
else{
cloud_projected_filtered = cloud_filtered_inc_walls;
}
ROS_INFO("After WF: %d", cloud_projected_filtered->width * cloud_projected_filtered->height);
return cloud_projected_filtered;
}
else
{
return cloud_filtered_inc_walls; // correct?
}
}
else
{
return cloud_ransac; // correct?
}
}
else
{
return cloud_ransac;
}
}
else
{
return cloud_filtered_z;
}
}
else
{
return cloud;
}
}
void cloud_cb (const pcl::PCLPointCloud2ConstPtr& cloud_blob)
{
std::cout<<"Received Kinect message\n";
pcl::PCLPointCloud2::Ptr cloud_filtered_blob (new pcl::PCLPointCloud2);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>), cloud_p (new pcl::PointCloud<pcl::PointXYZRGB>), cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>);
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloud_blob);
boost::this_thread::sleep (boost::posix_time::microseconds (100));
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*cloud_filtered_blob);
// Convert to the templated PointCloud
pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
// Create the segmentation object
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.01);
seg.setInputCloud (cloud_filtered);
int i = 0, nr_points = (int) cloud_filtered->points.size ();
pcl::IndicesPtr remaining (new std::vector<int>);
remaining->resize (nr_points);
for (size_t i = 0; i < remaining->size (); ++i) { (*remaining)[i] = static_cast<int>(i); }
std::cout << "here again" << std::endl;
// While 30% of the original cloud is still there
while (remaining->size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setIndices (remaining);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0) break;
// Extract the inliers
std::vector<int>::iterator it = remaining->begin();
for (size_t i = 0; i < inliers->indices.size (); ++i)
{
int curr = inliers->indices[i];
// Remove it from further consideration.
while (it != remaining->end() && *it < curr) { ++it; }
if (it == remaining->end()) break;
if (*it == curr) it = remaining->erase(it);
}
i++;
}
std::cout << "Found " << i << " planes." << std::endl;
// Color all the non-planar things.
for (std::vector<int>::iterator it = remaining->begin(); it != remaining->end(); ++it)
{
uint8_t r = 0, g = 255, b = 0;
uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
cloud_filtered->at(*it).rgb = *reinterpret_cast<float*>(&rgb);
}
pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
viewer.showCloud (cloud_filtered);
while (!viewer.wasStopped ())
{
}
// Publish the planes we found.
pcl::PCLPointCloud2 outcloud;
pcl::toPCLPointCloud2 (*cloud_filtered, outcloud);
pub.publish (outcloud);
}
int
main(int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZRGB>);
if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("../learn15.pcd", *cloud) == -1) //* load the file
{
PCL_ERROR("Couldn't read file test_pcd.pcd \n");
return (-1);
}
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients(true);
// Mandatory
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.01);
seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficients);
if (inliers->indices.size() == 0)
{
PCL_ERROR("Could not estimate a planar model for the given dataset.");
return (-1);
}
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud(cloud);
extract.setIndices(inliers);
extract.setNegative(true);
//Removes part_of_cloud but retain the original full_cloud
//extract.setNegative(true); // Removes part_of_cloud from full cloud and keep the rest
extract.filter(*cloud_plane);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
sor.setInputCloud(cloud_plane);
sor.setMeanK(50);
sor.setStddevMulThresh(1.0);
sor.filter(*cloud_filtered);
//cloud.swap(cloud_plane);
/*
pcl::visualization::CloudViewer viewer("Simple Cloud Viewer");
viewer.showCloud(cloud_plane);
while (!viewer.wasStopped())
{
}
*/
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
Eigen::Matrix<float, 1, 3> fitted_plane_norm, xyaxis_plane_norm, rotation_axis;
fitted_plane_norm[0] = coefficients->values[0];
fitted_plane_norm[1] = coefficients->values[1];
fitted_plane_norm[2] = coefficients->values[2];
xyaxis_plane_norm[0] = 0.0;
xyaxis_plane_norm[1] = 0.0;
xyaxis_plane_norm[2] = 1.0;
rotation_axis = xyaxis_plane_norm.cross(fitted_plane_norm);
float theta = acos(xyaxis_plane_norm.dot(fitted_plane_norm));
//float theta = -atan2(rotation_axis.norm(), xyaxis_plane_norm.dot(fitted_plane_norm));
transform_2.rotate(Eigen::AngleAxisf(theta, rotation_axis));
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_recentered(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::transformPointCloud(*cloud_filtered, *cloud_recentered, transform_2);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud_recentered, *cloud_xyz);
///////////////////////////////////////////////////////////////////
Eigen::Vector4f pcaCentroid;
pcl::compute3DCentroid(*cloud_xyz, pcaCentroid);
Eigen::Matrix3f covariance;
computeCovarianceMatrixNormalized(*cloud_xyz, pcaCentroid, covariance);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();
std::cout << eigenVectorsPCA.size() << std::endl;
if(eigenVectorsPCA.size()!=9)
eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1));
Eigen::Matrix4f projectionTransform(Eigen::Matrix4f::Identity());
projectionTransform.block<3, 3>(0, 0) = eigenVectorsPCA.transpose();
projectionTransform.block<3, 1>(0, 3) = -1.f * (projectionTransform.block<3, 3>(0, 0) * pcaCentroid.head<3>());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPointsProjected(new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*cloud_xyz, *cloudPointsProjected, projectionTransform);
// Get the minimum and maximum points of the transformed cloud.
pcl::PointXYZ minPoint, maxPoint;
pcl::getMinMax3D(*cloudPointsProjected, minPoint, maxPoint);
const Eigen::Vector3f meanDiagonal = 0.5f*(maxPoint.getVector3fMap() + minPoint.getVector3fMap());
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer = rgbVis(cloud);
// viewer->addPointCloud<pcl::PointXYZ>(cloudPointsProjected, "Simple Cloud");
// viewer->addPointCloud<pcl::PointXYZRGB>(cloud, "Simple Cloud2");
viewer->addCube(minPoint.x, maxPoint.x, minPoint.y, maxPoint.y, minPoint.z , maxPoint.z);
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
/*
pcl::PCA< pcl::PointXYZ > pca;
pcl::PointCloud< pcl::PointXYZ >::ConstPtr cloud;
pcl::PointCloud< pcl::PointXYZ > proj;
pca.setInputCloud(cloud);
pca.project(*cloud, proj);
Eigen::Vector4f proj_min;
Eigen::Vector4f proj_max;
pcl::getMinMax3D(proj, proj_min, proj_max);
pcl::PointXYZ min;
pcl::PointXYZ max;
pca.reconstruct(proj_min, min);
pca.reconstruct(proj_max, max);
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer->addCube(min.x, max.x, min.y, max.y, min.z, max.z);
*/
return (0);
}
void point_cb(const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PCLPointCloud2 cloud_filtered;
pcl_conversions::toPCL(*cloud_msg, *cloud);
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud(cloudPtr);
float leaf = 0.005;
sor.setLeafSize(leaf, leaf, leaf);
sor.filter(cloud_filtered);
sensor_msgs::PointCloud2 sensor_pcl;
pcl_conversions::moveFromPCL(cloud_filtered, sensor_pcl);
//publish pcl data
pub_voxel.publish(sensor_pcl);
global_counter++;
if((theta == 0.0 && y_offset == 0.0) || global_counter < 800 ){
// part for planar segmentation starts here ..
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>), cloud_p(new pcl::PointCloud<pcl::PointXYZ>), cloud_seg(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p_rotated(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p_transformed(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZ>);
sensor_msgs::PointCloud2 plane_sensor, plane_transf_sensor;
//convert sen
pcl::fromROSMsg(*cloud_msg, *cloud1);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.01);
seg.setInputCloud(cloud1);
seg.segment (*inliers, *coefficients);
Eigen::Matrix<float, 1, 3> surface_normal;
Eigen::Matrix<float, 1, 3> floor_normal;
surface_normal[0] = coefficients->values[0];
surface_normal[1] = coefficients->values[1];
surface_normal[2] = coefficients->values[2];
std::cout << surface_normal[0] << "\n" << surface_normal[1] << "\n" << surface_normal[2];
floor_normal[0] = 0.0;
floor_normal[1] = 1.0;
floor_normal[2] = 0.0;
theta = acos(surface_normal.dot(floor_normal));
//extract the inliers - copied from tutorials...
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud1);
extract.setIndices (inliers);
extract.setNegative(true);
extract.filter(*cloud_p);
ROS_INFO("print cloud info %d", cloud_p->height);
pcl::toROSMsg(*cloud_p, plane_sensor);
pub_plane_simple.publish(plane_sensor);
// anti rotate the point cloud by first finding the angle of rotation
Eigen::Affine3f transform_1 = Eigen::Affine3f::Identity();
transform_1.translation() << 0.0, 0.0, 0.0;
transform_1.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));
pcl::transformPointCloud(*cloud_p, *cloud_p_rotated, transform_1);
double y_sum = 0;
// int num_points =
for (int i = 0; i < 20; i++){
y_sum = cloud_p_rotated->points[i].y;
}
y_offset = y_sum / 20;
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
transform_2.translation() << 0.0, -y_offset, 0.0;
transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));
pcl::transformPointCloud(*cloud_p, *cloud_p_transformed, transform_2);
pcl::transformPointCloud(*cloud1, *cloud_transformed, transform_2);
//now remove the y offset because of the camera rotation
pcl::toROSMsg(*cloud_p_transformed, plane_transf_sensor);
// pcl::toROSMsg(*cloud_transformed, plane_transf_sensor);
// pcl::toROSMsg(*cloud1, plane_transf_sensor);
pub_plane_transf.publish(plane_transf_sensor);
}
ras_msgs::Cam_transform cft;
cft.theta = theta;
cft.y_offset = y_offset;
pub_ctf.publish(cft);
// pub_tf.publish();
}
// Ros kinect topic callback
void callback(const sensor_msgs::PointCloud2ConstPtr& input) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudKinect(new pcl::PointCloud<pcl::PointXYZ>());
pcl::fromROSMsg(*input, *cloudKinect);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>());
//std::cout << cloud << std::endl;
//ROS_INFO(input);
//Inside the callback should be all the process that needed to be done with the point cloud
//pcl::PointCloud<pcl::PointXYZ> cloudKinect;
//pcl::fromROSMsg(*input, cloudKinect);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(cloudKinect);
//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
std::cout << cloud->points.size () << std::endl;
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PointXYZ> vg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>());
vg.setInputCloud (cloud);
vg.setLeafSize (0.01f, 0.01f, 0.01f);
vg.filter (*cloud_filtered);
std::cout << *cloud << std::endl;
// pcl::PassThrough<pcl::PointXYZ> pass;
// pass.setInputCloud (cloud);
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PCDWriter writer;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
int i=0, nr_points = (int) cloud_filtered->points.size ();
while (cloud_filtered->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
// Get the points associated with the planar surface
extract.filter (*cloud_plane);
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
*cloud_filtered = *cloud_f;
}
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud_filtered);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
float minX = 1000000.0;
float minY = 1000000.0;
float maxX = -1000000.0;
float maxY = -1000000.0;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++){
pcl::PointXYZ p = cloud_filtered->points[*pit];
cloud_cluster->points.push_back (cloud_filtered->points[*pit]);
if(p.x < minX){ minX = p.x; }
if(p.x > maxX){ maxX = p.x; }
if(p.y < minY){ minY = p.y; }
if(p.y > maxY){ maxY = p.y; }
}
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*cloud_cluster, centroid);
// for (pcl::PointCloud<pcl::PointXYZ>::iterator p = cloud_cluster->points.begin(); p < cloud_cluster->points.end(); p++)
// {
// if(p.x < minX){ minX = p; }
// if(p.x > maxX){ maxX = p; }
// if(p.y < minY){ minY = p; }
// if(p.y > maxY){ maxY = p; }
// }
// std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
// std::cout << "Centroid " << centroid[0] << "," << centroid[1] << "," << centroid[2] << "." << std::endl;
// std::cout << "min x: " << minX << std::endl;
// std::cout << "max x: " << maxX << std::endl;
// std::cout << "min y: " << minY << std::endl;
// std::cout << "max y: " << maxY << std::endl;
float xdist = std::abs(minX-maxY);
float ydist = std::abs(minY-maxY);
// std::cout << "dist x: " << xdist << std::endl;
// std::cout << "dist y: " << ydist << std::endl;
if(xdist < grippersize || ydist < grippersize){
// std::cout << "Graspable" << std::endl;
std_msgs::String msg;
std::stringstream rosmsg;
rosmsg << centroid[0] << ",";
rosmsg << centroid[1] << ",";
rosmsg << centroid[2];
msg.data = rosmsg.str();
visionPublisher.publish(msg);
std::stringstream ss;
ss << "graspable_object" << j << ".pcd";
writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false);
j++;
}
}
}
int
main(int argc, char** argv)
{
// Objects for storing the point clouds.
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudNoPlane(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr planePoints(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr projectedPoints(new pcl::PointCloud<pcl::PointXYZ>);
// Read a PCD file from disk.
if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) != 0)
{
return -1;
}
// Get the plane model, if present.
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::SACSegmentation<pcl::PointXYZ> segmentation;
segmentation.setInputCloud(cloud);
segmentation.setModelType(pcl::SACMODEL_PLANE);
segmentation.setMethodType(pcl::SAC_RANSAC);
segmentation.setDistanceThreshold(0.01);
segmentation.setOptimizeCoefficients(true);
pcl::PointIndices::Ptr inlierIndices(new pcl::PointIndices);
segmentation.segment(*inlierIndices, *coefficients);
if (inlierIndices->indices.size() == 0)
std::cout << "Could not find a plane in the scene." << std::endl;
else
{
std::cerr << "Plane coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
// Create a second point cloud that does not have the plane points.
// Also, extract the plane points to visualize them later.
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud);
extract.setIndices(inlierIndices);
extract.filter(*planePoints);
extract.setNegative(true);
extract.filter(*cloudNoPlane);
// Object for projecting points onto a model.
pcl::ProjectInliers<pcl::PointXYZ> projection;
// We have to specify what model we used.
projection.setModelType(pcl::SACMODEL_PLANE);
projection.setInputCloud(cloudNoPlane);
// And we have to give the coefficients we got.
projection.setModelCoefficients(coefficients);
projection.filter(*projectedPoints);
// Visualize everything.
pcl::visualization::CloudViewer viewerPlane("Plane");
viewerPlane.showCloud(planePoints);
while (!viewerPlane.wasStopped())
{
// Do nothing but wait.
}
pcl::visualization::CloudViewer viewerProjection("Projected points");
viewerProjection.showCloud(projectedPoints);
while (!viewerProjection.wasStopped())
{
// Do nothing but wait.
}
}
}
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::io::loadPCDFile(argv[1],cloud);
//pcl::io::loadPLYFile(argv[1], cloud);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
double distanceTh = 0.025;
if(argc > 2)
distanceTh = atof(argv[2]);
seg.setDistanceThreshold (distanceTh);
seg.setInputCloud (cloud.makeShared ());
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
PCL_ERROR ("Could not estimate a planar model for the given dataset.");
return (-1);
}
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
pcl::PointCloud<pcl::PointXYZ> output;
output.width = inliers->indices.size();
output.height = 1;
output.points.resize(output.width * output.height);
std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;
for (size_t i = 0; i < inliers->indices.size (); ++i){
/*std::cerr << inliers->indices[i] << " " << cloud.points[inliers->indices[i]].x << " "
<< cloud.points[inliers->indices[i]].y << " "
<< cloud.points[inliers->indices[i]].z << std::endl;*/
output.points[i].x = cloud.points[inliers->indices[i]].x;
output.points[i].y = cloud.points[inliers->indices[i]].y;
output.points[i].z = cloud.points[inliers->indices[i]].z;
}
pcl::PointCloud<pcl::PointXYZ> object;
pcl::ExtractIndices<pcl::PointXYZ> extract;
pcl::PointCloud<pcl::PointXYZ>::Ptr tempPtr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(cloud,*tempPtr);
extract.setInputCloud(tempPtr);
extract.setIndices(inliers);
extract.setNegative(true);
extract.filter(object);
pcl::io::savePCDFile("output.pcd",output);
pcl::io::savePCDFile("object.pcd",object);
/*THE PLANAR SEGMENT REMOVED NOW RUN CLUSTERING AND COMPARE WITH EACH ONE.*/
system("../eucliden_clustering/build/cluster_extraction ./object.pcd");
return (0);
}
void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input) {
// std::cerr << "in cloud_cb" << std::endl;
/* 0. Importing input cloud */
std_msgs::Header header = input->header;
// std::string frame_id = input->header.frame_id;
// sensor_msgs::PointCloud2 input_cloud = *input;
pcl::PCLPointCloud2 *cloud = new pcl::PCLPointCloud2; // initialize object
pcl_conversions::toPCL(*input, *cloud); // from input, generate content of cloud
/* 1. Downsampling and Publishing voxel */
// LeafSize: should small enough to caputure a leaf of plants
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud); // imutable pointer
pcl::PCLPointCloud2 cloud_voxel; // cloud_filtered to cloud_voxel
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud(cloudPtr); // set input
sor.setLeafSize(0.02f, 0.02f, 0.02f); // 2cm, model equation
sor.filter(cloud_voxel); // set output
sensor_msgs::PointCloud2 output_voxel;
pcl_conversions::fromPCL(cloud_voxel, output_voxel);
pub_voxel.publish(output_voxel);
/* 2. Filtering with RANSAC */
// RANSAC initialization
pcl::SACSegmentation<pcl::PointXYZ> seg; // Create the segmentation object
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
seg.setOptimizeCoefficients(true); // Optional
// seg.setModelType(pcl::SACMODEL_PLANE);
seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE); // Use SACMODEL_PERPENDICULAR_PLANE instead
seg.setMethodType(pcl::SAC_RANSAC);
// minimum number of points calculated from N and distanceThres
seg.setMaxIterations (1000); // N in RANSAC
// seg.setDistanceThreshold(0.025); // 0.01 - 0.05 default: 0.02 // 閾値（しきい値）
seg.setDistanceThreshold(0.050); // 0.01 - 0.05 default: 0.02 // 閾値（しきい値）
// param for perpendicular
seg.setAxis(Eigen::Vector3f (0.0, 0.0, 1.0));
seg.setEpsAngle(pcl::deg2rad (10.0f)); // 5.0 -> 10.0
// convert from PointCloud2 to PointXYZ
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_voxel_xyz (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(cloud_voxel, *cloud_voxel_xyz);
// RANSAC application
seg.setInputCloud(cloud_voxel_xyz);
seg.segment(*inliers, *coefficients); // values are empty at beginning
// inliers.indices have array index of the points which are included as inliers
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane_xyz (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = inliers->indices.begin ();
pit != inliers->indices.end (); pit++) {
cloud_plane_xyz->points.push_back(cloud_voxel_xyz->points[*pit]);
}
// Organized as an image-structure
cloud_plane_xyz->width = cloud_plane_xyz->points.size ();
cloud_plane_xyz->height = 1;
/* insert code to set arbitary frame_id setting
such as frame_id ="/assemble_cloud_1"
with respect to "/odom or /base_link" */
// Conversions: PointCloud<T>, PCLPointCloud2, sensor_msgs::PointCloud2
pcl::PCLPointCloud2 cloud_plane_pcl;
pcl::toPCLPointCloud2(*cloud_plane_xyz, cloud_plane_pcl);
sensor_msgs::PointCloud2 cloud_plane_ros;
pcl_conversions::fromPCL(cloud_plane_pcl, cloud_plane_ros);
// cloud_plane_ros.header.frame_id = "/base_link"; // odom -> /base_link
cloud_plane_ros.header.frame_id = header.frame_id;
cloud_plane_ros.header.stamp = header.stamp; // ros::Time::now() -> header.stamp
pub_plane.publish(cloud_plane_ros);
/* 3. PCA application to get eigen */
pcl::PCA<pcl::PointXYZ> pca;
pca.setInputCloud(cloud_plane_xyz);
Eigen::Matrix3f eigen_vectors = pca.getEigenVectors(); // 3x3 eigen_vectors(n,m)
/* 4. PCA Visualization */
visualization_msgs::Marker points;
// points.header.frame_id = "/base_link"; // odom -> /base_link
points.header.frame_id = header.frame_id;
points.header.stamp = input->header.stamp; // ros::Time::now() -> header.stamp
points.ns = "pca"; // namespace + id
points.id = 0; // pca/0
points.action = visualization_msgs::Marker::ADD;
points.type = visualization_msgs::Marker::ARROW;
points.pose.orientation.w = 1.0;
points.scale.x = 0.05;
points.scale.y = 0.05;
points.scale.z = 0.05;
points.color.g = 1.0f;
points.color.r = 0.0f;
points.color.b = 0.0f;
points.color.a = 1.0;
geometry_msgs::Point p_0, p_1;
p_0.x = 0; p_0.y = 0; p_0.z = 0; // get from tf
p_1.x = eigen_vectors(0,0);
p_1.y = eigen_vectors(0,1); // always negative
std::cerr << "y = " << eigen_vectors(0,1) << std::endl;
p_1.z = eigen_vectors(0,2);
points.points.push_back(p_0);
points.points.push_back(p_1);
pub_marker.publish(points);
/* 5. Point Cloud Rotation */
eigen_vectors(0,2) = 0; // ignore very small z-value
double norm = pow((pow(eigen_vectors(0,0), 2) + pow(eigen_vectors(0,1), 2)), 0.5);
double nx = eigen_vectors(0,0) / norm;
double ny = eigen_vectors(0,1) / norm;
Eigen::Matrix4d rot_z; // rotation inversed, convert to Matrix4f
rot_z(0,0) = nx; rot_z(0,1) = ny; rot_z(0,2) = 0; rot_z(0,3) = 0; // ny: +/-
rot_z(1,0) = -ny; rot_z(1,1) = nx; rot_z(1,2) = 0; rot_z(1,3) = 0; // ny: +/-
rot_z(2,0) = 0; rot_z(2,1) = 0; rot_z(2,2) = 1; rot_z(2,3) = 0;
rot_z(3,0) = 0; rot_z(3,1) = 0; rot_z(3,2) = 0; rot_z(3,3) = 1;
// Transformation: Rotation, Translation
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_rot (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(*cloudPtr, *cloud_xyz); // from PointCloud2 to PointXYZ
pcl::transformPointCloud(*cloud_xyz, *cloud_xyz_rot, rot_z); // original, transformed, transformation
pcl::PCLPointCloud2 cloud_rot_pcl;
sensor_msgs::PointCloud2 cloud_rot_ros;
pcl::toPCLPointCloud2(*cloud_xyz_rot, cloud_rot_pcl);
pcl_conversions::fromPCL(cloud_rot_pcl, cloud_rot_ros);
pub_rot.publish(cloud_rot_ros);
/* 6. Point Cloud Reduction */
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr >
vector_cloud_separated_xyz = separate(cloud_xyz_rot, header);
pcl::PCLPointCloud2 cloud_separated_pcl;
sensor_msgs::PointCloud2 cloud_separated_ros;
pcl::toPCLPointCloud2(*vector_cloud_separated_xyz.at(1), cloud_separated_pcl);
pcl_conversions::fromPCL(cloud_separated_pcl, cloud_separated_ros);
// cloud_separated_ros.header.frame_id = "/base_link"; // odom -> /base_link
cloud_separated_ros.header.frame_id = header.frame_id;
cloud_separated_ros.header.stamp = input->header.stamp; // ros::Time::now() -> header.stamp
pub_red.publish(cloud_separated_ros);
// setMarker
// visualization_msgs::Marker width_min_line;
// width_min_line.header.frame_id = "/base_link";
// width_min_line.header.stamp = input->header.stamp; // ros::Time::now() -> header.stamp
// width_min_line.ns = "width_min";
// width_min_line.action = visualization_msgs::Marker::ADD;
// width_min_line.type = visualization_msgs::Marker::LINE_STRIP;
// width_min_line.pose.orientation.w = 1.0;
// width_min_line.id = 0;
// width_min_line.scale.x = 0.025;
// width_min_line.color.r = 0.0f;
// width_min_line.color.g = 1.0f;
// width_min_line.color.b = 0.0f;
// width_min_line.color.a = 1.0;
// width_min_line.points.push_back(point_vector.at(0));
// width_min_line.points.push_back(point_vector.at(2));
// pub_marker.publish(width_min_line);
// /* 6. Visualize center line */
// visualization_msgs::Marker line_strip;
// line_strip.header.frame_id = "/base_link"; // odom -> /base_link
// line_strip.header.stamp = input->header.stamp; // ros::Time::now() -> header.stamp
// line_strip.ns = "center";
// line_strip.action = visualization_msgs::Marker::ADD;
// line_strip.type = visualization_msgs::Marker::LINE_STRIP;
// line_strip.pose.orientation.w = 1.0;
// line_strip.id = 0; // set id
// line_strip.scale.x = 0.05;
// line_strip.color.r = 1.0f;
// line_strip.color.g = 0.0f;
// line_strip.color.b = 0.0f;
// line_strip.color.a = 1.0;
// // geometry_msgs::Point p_stitch, p_min;
// p_s.x = 0; p_s.y = 0; p_s.z = 0;
// p_e.x = p_m.x; p_e.y = p_m.y; p_e.z = 0;
// line_strip.points.push_back(p_s);
// line_strip.points.push_back(p_e);
// pub_marker.publish(line_strip);
/* PCA Visualization */
// geometry_msgs::Pose pose; tf::poseEigenToMsg(pca.getEigenVectors, pose);
/* to use Pose marker in rviz */
/* Automatic Measurement */
// 0-a. stitch measurement: -0.5 < z < -0.3
// 0-b. min width measurement: 0.3 < z < 5m
// 1. iterate
// 2. pick point if y < 0
// 3. compare point with all points if 0 < y
// 4. pick point-pare recording shortest distance
// 5. compare the point with previous point
// 6. update min
// 7. display value in text in between 2 points
}
