void App::Load() {
Input_.reset(new PointCloud);
pcl::io::loadPCDFile(InputPath_, *Input_);
}
void GraphicEnd::downsamplePointCloud(PointCloud::Ptr& pc_in,PointCloud::Ptr& pc_downsampled)
{
if(use_voxel)
{
pcl::VoxelGrid<pcl::PointXYZRGB> grid;
grid.setLeafSize(0.05,0.05,0.05);
grid.setFilterFieldName ("z");
grid.setFilterLimits (0.0,5.0);
grid.setInputCloud(pc_in);
grid.filter(*pc_downsampled);
}
else
{
int downsamplingStep=8;
static int j;j=0;
std::vector<double> xV;
std::vector<double> yV;
std::vector<double> zV;
std::vector<double> rV;
std::vector<double> gV;
std::vector<double> bV;
pc_downsampled.reset(new pcl::PointCloud<pcl::PointXYZRGB> );
pc_downsampled->points.resize(640*480/downsamplingStep*downsamplingStep);
for(int r=0;r<480;r=r+downsamplingStep)
{
for(int c=0;c<640;c=c+downsamplingStep)
{
int nPoints=0;
xV.resize(downsamplingStep*downsamplingStep);
yV.resize(downsamplingStep*downsamplingStep);
zV.resize(downsamplingStep*downsamplingStep);
rV.resize(downsamplingStep*downsamplingStep);
gV.resize(downsamplingStep*downsamplingStep);
bV.resize(downsamplingStep*downsamplingStep);
for(int r2=r;r2<r+downsamplingStep;r2++)
{
for(int c2=c;c2<c+downsamplingStep;c2++)
{
//Check if the point has valid data
if(pcl_isfinite (pc_in->points[r2*640+c2].x) &&
pcl_isfinite (pc_in->points[r2*640+c2].y) &&
pcl_isfinite (pc_in->points[r2*640+c2].z) &&
0.3<pc_in->points[r2*640+c2].z &&
pc_in->points[r2*640+c2].z<5)
{
//Create a vector with the x, y and z coordinates of the square region and RGB info
xV[nPoints]=pc_in->points[r2*640+c2].x;
yV[nPoints]=pc_in->points[r2*640+c2].y;
zV[nPoints]=pc_in->points[r2*640+c2].z;
rV[nPoints]=pc_in->points[r2*640+c2].r;
gV[nPoints]=pc_in->points[r2*640+c2].g;
bV[nPoints]=pc_in->points[r2*640+c2].b;
nPoints++;
}
}
}
if(nPoints>0)
{
xV.resize(nPoints);
yV.resize(nPoints);
zV.resize(nPoints);
rV.resize(nPoints);
gV.resize(nPoints);
bV.resize(nPoints);
//Compute the mean 3D point and mean RGB value
std::sort(xV.begin(),xV.end());
std::sort(yV.begin(),yV.end());
std::sort(zV.begin(),zV.end());
std::sort(rV.begin(),rV.end());
std::sort(gV.begin(),gV.end());
std::sort(bV.begin(),bV.end());
pcl::PointXYZRGB point;
point.x=xV[nPoints/2];
point.y=yV[nPoints/2];
point.z=zV[nPoints/2];
point.r=rV[nPoints/2];
point.g=gV[nPoints/2];
point.b=bV[nPoints/2];
//Set the mean point as the representative point of the region
pc_downsampled->points[j]=point;
j++;
}
}
}
pc_downsampled->points.resize(j);
pc_downsampled->width=pc_downsampled->size();
pc_downsampled->height=1;
}
}
std::vector<PointCloud> TableClusterDetector::findTableClusters(const sensor_msgs::PointCloud2 &scene)
{
std::vector<PointCloud> clusters;
// Convert the dataset
PointCloud cloud; PointCloud::Ptr cloudPtr;
pcl::fromROSMsg (scene, cloud);
cloudPtr.reset(new PointCloud(cloud));
// Remove NaNs
PointCloud cloud_filtered;
pass_.setInputCloud (cloudPtr);
pass_.filter (cloud_filtered);
cloudPtr.reset(new PointCloud(cloud_filtered));
// Downsample
PointCloud cloud_downsampled;
grid_.setInputCloud (cloudPtr);
grid_.filter (cloud_downsampled);
cloudPtr.reset(new PointCloud(cloud_downsampled));
if ((int)cloud_filtered.points.size() < k_)
{
ROS_WARN("Filtering returned %zd points! Skipping.", cloud_filtered.points.size());
return clusters;
}
// Estimate the point normals
pcl::PointCloud<pcl::Normal> cloud_normals;pcl::PointCloud<pcl::Normal>::Ptr cloud_normalsPtr;
// add this if normal estimation is inaccurate
//n3d_.setSearchSurface (cloud_);
n3d_.setInputCloud (cloudPtr);
n3d_.compute (cloud_normals);
cloud_normalsPtr.reset(new pcl::PointCloud<pcl::Normal>(cloud_normals));
ROS_INFO ("[TableObjectDetector] %d normals estimated.", (int)cloud_normals.points.size ());
// ---[ Perform segmentation
pcl::PointIndices table_inliers; pcl::PointIndices::Ptr table_inliersPtr;
pcl::ModelCoefficients table_coefficients; pcl::ModelCoefficients::Ptr table_coefficientsPtr;
seg_.setInputCloud (cloudPtr);
seg_.setInputNormals (cloud_normalsPtr);
seg_.segment (table_inliers, table_coefficients);
table_inliersPtr = boost::make_shared<pcl::PointIndices>(table_inliers);
table_coefficientsPtr = boost::make_shared<pcl::ModelCoefficients>(table_coefficients);
if (table_coefficients.values.size () > 3)
ROS_INFO ("[TableObjectDetector::input_callback] Model found with %d inliers: [%f %f %f %f].", (int)table_inliers.indices.size (),
table_coefficients.values[0], table_coefficients.values[1], table_coefficients.values[2], table_coefficients.values[3]);
if (table_inliers.indices.size () == 0)
return clusters;
// ---[ Extract the table
PointCloud table_projected; PointCloud::Ptr table_projectedPtr;
proj_.setInputCloud (cloudPtr);
proj_.setIndices (table_inliersPtr);
proj_.setModelCoefficients (table_coefficientsPtr);
proj_.filter (table_projected);
table_projectedPtr.reset (new PointCloud(table_projected));
ROS_INFO ("[TableObjectDetector::input_callback] Number of projected inliers: %d.", (int)table_projected.points.size ());
sensor_msgs::PointCloud table_points;
tf::Transform table_plane_trans = getPlaneTransform (*table_coefficientsPtr, -1.0);
std::string base_frame_id = scene.header.frame_id;
ROS_INFO("sending table transform");
ros::Rate r(10);
for (int i=0; i < 10; i++) {
tf_pub_.sendTransform(tf::StampedTransform(table_plane_trans, ros::Time::now(),
base_frame_id, "table"));
r.sleep();
}
//takes the points projected on the table and transforms them into the PointCloud message
//while also transforming them into the table's coordinate system
getPlanePoints<Point> (table_projected, table_plane_trans, table_points);
ROS_INFO("Table computed");
table_ = computeTable<sensor_msgs::PointCloud>(cloud.header, table_plane_trans, table_points);
publishTable(table_);
// ---[ Estimate the convex hull
PointCloud table_hull; PointCloud::Ptr table_hullPtr;
hull_.setInputCloud (table_projectedPtr);
hull_.reconstruct (table_hull);
table_hullPtr.reset (new PointCloud(table_hull));
// ---[ Get the objects on top of the table
pcl::PointIndices cloud_object_indices; pcl::PointIndices::Ptr cloud_object_indicesPtr;
prism_.setInputCloud (cloudPtr);
prism_.setInputPlanarHull (table_hullPtr);
prism_.segment (cloud_object_indices);
cloud_object_indicesPtr = boost::make_shared<pcl::PointIndices>(cloud_object_indices);
ROS_INFO ("[TableObjectDetector::input_callback] Number of object point indices: %d.", (int)cloud_object_indices.indices.size ());
PointCloud cloud_objects; PointCloud::Ptr cloud_objectsPtr;
pcl::ExtractIndices<Point> extract_object_indices;
extract_object_indices.setInputCloud (cloudPtr);
extract_object_indices.setIndices (cloud_object_indicesPtr);
extract_object_indices.filter (cloud_objects);
cloudPtr.reset(new PointCloud(cloud_objects));
ROS_INFO ("[TableObjectDetector::input_callback] Number of object point candidates: %d.", (int)cloud_objects.points.size ());
if (cloud_objects.points.size () == 0)
return clusters;
// ---[ Downsample the points
PointCloud cloud_objects_downsampled;PointCloud::Ptr cloud_objects_downsampledPtr;
grid_objects_.setInputCloud (cloudPtr);
grid_objects_.filter (cloud_objects_downsampled);
cloudPtr.reset (new PointCloud(cloud_objects_downsampled));
ROS_INFO ("[TableObjectDetector::input_callback] Number of object point candidates left after downsampling: %d.", (int)cloud_objects_downsampled.points.size ());
// ---[ Split the objects into Euclidean clusters
std::vector<pcl::PointIndices> clustersIndices;
cluster_.setInputCloud (cloudPtr);
cluster_.extract (clustersIndices);
ROS_INFO ("[TableObjectDetector::input_callback] Number of clusters found matching the given constraints: %d.", (int)clustersIndices.size ());
AffineTransform table_trans_eig;
cse481::tfToEigen(table_plane_trans, table_trans_eig);
table_trans_eig = table_trans_eig.inverse().eval();
PointCloud cloud_in_table_frame;
pcl::transformPointCloud(cloud_objects_downsampled, cloud_in_table_frame, table_trans_eig);
// Clouds are now in table frame
BOOST_FOREACH(pcl::PointIndices indices, clustersIndices) {
PointCloud clusterCloud;
pcl::copyPointCloud(cloud_in_table_frame, indices, clusterCloud);
clusters.push_back(clusterCloud);
}