void QNode::cloudCallback(const sensor_msgs::PointCloud2ConstPtr &cloud_msg){
// Convert ROS message (PointCloud2) to PCL point cloud (PointCloud(PointXYZ))
pcl::fromROSMsg(*cloud_msg, cloud);
if(rgb_enabled){
pcl::fromROSMsg(*cloud_msg, cloudRGB);
}
// Cloud conversion and visualization
pcl::PointCloud<pcl::PointXYZ>::Ptr tmpCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*cloud_msg, *tmpCloud);
picture_flag = true;
Q_EMIT setPointCloud(tmpCloud);
}
int
process(const tendrils& i, const tendrils& o)
{
// //clean up the model
std::vector<unsigned int> removed;
surfels::finalCleanup(*model, *params, removed);
//convert to cloud
pcl::PointCloud<surfels::surfelPt>::Ptr surfelCloud(new pcl::PointCloud<surfels::surfelPt>());
surfels::convertModelToCloud(*model, *surfelCloud);
//passthrough filter (remove anything at or below table height
pcl::PointCloud<surfels::surfelPt>::Ptr tmpCloud(new pcl::PointCloud<surfels::surfelPt>());
// pcl::PassThrough<surfels::surfelPt> pass;
// pass.setInputCloud(surfelCloud);
// pass.setFilterFieldName("z");
// pass.setFilterLimits(0.001, 1.0);
// pass.filter(*tmpCloud);
// surfelCloud.swap(tmpCloud);
//statistical outlier filter
pcl::StatisticalOutlierRemoval<surfels::surfelPt> sor;
sor.setInputCloud(surfelCloud);
sor.setMeanK(50);
sor.setStddevMulThresh(2.0);
sor.filter(*tmpCloud);
surfelCloud.swap(tmpCloud);
//try to determine the dimensions of the base
float minX = 0, minY = 0, maxX = 0, maxY = 0;
pcl::PassThrough<surfels::surfelPt> basePass;
basePass.setInputCloud(surfelCloud);
basePass.setFilterFieldName("z");
basePass.setFilterLimits(0.001, 0.02);
basePass.filter(*tmpCloud);
for (unsigned int i = 0; i < tmpCloud->points.size(); i++)
{
surfels::surfelPt const& pt = tmpCloud->points[i];
if (i == 0 || pt.x < minX)
minX = pt.x;
if (i == 0 || pt.x > maxX)
maxX = pt.x;
if (i == 0 || pt.y < minY)
minY = pt.y;
if (i == 0 || pt.y > maxY)
maxY = pt.y;
}
float maxRadius = fabs(minX);
if (maxX < maxRadius)
maxRadius = maxX;
if (fabs(minY) < maxRadius)
maxRadius = fabs(minY);
if (maxY < maxRadius)
maxRadius = maxY;
//fake seeing the bottom so the reconstruction won't have a lumpy bottom
surfels::surfelPt origin, tmp;
origin.x = origin.y = origin.z = 0;
origin.normal_x = origin.normal_y = 0;
origin.normal_z = -1.0;
surfelCloud->points.push_back(origin);
surfelCloud->width++;
float radius_inc = 0.002;
for (float radius = radius_inc; radius < maxRadius; radius += radius_inc)
{
float angle_inc = M_PI / (400 * radius);
for (float theta = 0; theta < 2 * M_PI; theta += angle_inc)
{
tmp = origin;
tmp.x += radius * cos(theta);
tmp.y += radius * sin(theta);
surfelCloud->points.push_back(tmp);
surfelCloud->width++;
}
}
//save as ply
//rgbd::write_ply_file(*surfelCloud,out_file);
writePLY(*surfelCloud, *filename);
return ecto::OK;
}
