//-----------------------------------------------------------------------------
//
// CloudQ series
//
//-----------------------------------------------------------------------------
void mglGraph::Cloud(const mglData &x, const mglData &y, const mglData &z, const mglData &a,
const char *sch, mreal alpha)
{
long i,j,k,n=a.nx,m=a.ny,l=a.nz;
register int i0,i1;
if(n<2 || m<2 || l<2) { SetWarn(mglWarnLow,"CloudQ"); return; }
bool both = x.nx*x.ny*x.nz==n*m*l && y.nx*y.ny*y.nz==n*m*l && z.nx*z.ny*z.nz==n*m*l;
if(!(both || (x.nx==n && y.nx==m && z.nx==l)))
{ SetWarn(mglWarnDim); return; }
static int cgid=1; StartGroup("CloudQ",cgid++);
int tx=1,ty=1,tz=1;
if(MeshNum>1)
{ tx=(n-1)/(MeshNum-1); ty=(m-1)/(MeshNum-1); tz=(l-1)/(MeshNum-1);}
if(tx<1) tx=1; if(ty<1) ty=1; if(tz<1) tz=1;
if(alpha<0) alpha = AlphaDef;
if(sch && strchr(sch,'-')) alpha = -alpha;
alpha /= pow(n/tx*m/ty*l/tz,1./3)/CloudFactor/5;
if(alpha>1) alpha = 1;
SetScheme(sch);
mreal *pp = new mreal[3*n*m*l];
if(!pp) { SetWarn(mglWarnMem); return; }
mreal *aa = new mreal[n*m*l];
// x, y -- матрицы как и z
if(both) for(i=0;i<n/tx;i++) for(j=0;j<m/ty;j++) for(k=0;k<l/tz;k++)
{
i0 = i+(n/tx)*(j+(m/ty)*k);
i1 = i*tx+n*(j*ty+m*k*tz);
pp[3*i0] = x.a[i1];
pp[3*i0+1]= y.a[i1];
pp[3*i0+2]= z.a[i1];
aa[i0] = a.a[i1];
}
// x, y -- вектора
else for(i=0;i<n/tx;i++) for(j=0;j<m/ty;j++) for(k=0;k<l/tz;k++)
{
i0 = i+(n/tx)*(j+(m/ty)*k);
pp[3*i0] = x.a[i*tx];
pp[3*i0+1]= y.a[j*ty];
pp[3*i0+2]= z.a[k*tz];
aa[i0] = a.a[i*tx+n*(j*ty+m*k*tz)];
}
bool al=Alpha(true);
cloud_plot(n/tx,m/ty,l/tz,pp,aa,alpha);
Alpha(al);
EndGroup();
delete []pp; delete []aa;
}
std::vector<GraspHypothesis> Localization::localizeHands(const PointCloud::Ptr& cloud_in, int size_left,
const std::vector<int>& indices, bool calculates_antipodal, bool uses_clustering)
{
double t0 = omp_get_wtime();
std::vector<GraspHypothesis> hand_list;
if (size_left == 0 || cloud_in->size() == 0)
{
//std::cout << "Input cloud is empty!\n";
//std::cout << size_left << std::endl;
hand_list.resize(0);
return hand_list;
}
// set camera source for all points (0 = left, 1 = right)
//std::cout << "Generating camera sources for " << cloud_in->size() << " points ...\n";
Eigen::VectorXi pts_cam_source(cloud_in->size());
if (size_left == cloud_in->size())
pts_cam_source << Eigen::VectorXi::Zero(size_left);
else
pts_cam_source << Eigen::VectorXi::Zero(size_left), Eigen::VectorXi::Ones(cloud_in->size() - size_left);
// remove NAN points from the cloud
std::vector<int> nan_indices;
pcl::removeNaNFromPointCloud(*cloud_in, *cloud_in, nan_indices);
// reduce point cloud to workspace
//std::cout << "Filtering workspace ...\n";
PointCloud::Ptr cloud(new PointCloud);
filterWorkspace(cloud_in, pts_cam_source, cloud, pts_cam_source);
//std::cout << " " << cloud->size() << " points left\n";
// store complete cloud for later plotting
PointCloud::Ptr cloud_plot(new PointCloud);
*cloud_plot = *cloud;
*cloud_ = *cloud;
// voxelize point cloud
//std::cout << "Voxelizing point cloud\n";
double t1_voxels = omp_get_wtime();
voxelizeCloud(cloud, pts_cam_source, cloud, pts_cam_source, 0.003);
double t2_voxels = omp_get_wtime() - t1_voxels;
//std::cout << " Created " << cloud->points.size() << " voxels in " << t2_voxels << " sec\n";
// plot camera source for each point in the cloud
if (plots_camera_sources_)
plot_.plotCameraSource(pts_cam_source, cloud);
if (uses_clustering)
{
//std::cout << "Finding point cloud clusters ... \n";
// 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>());
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(100);
seg.setDistanceThreshold(0.01);
// Segment the largest planar component from the remaining cloud
seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficients);
if (inliers->indices.size() == 0)
{
//std::cout << " Could not estimate a planar model for the given dataset." << std::endl;
hand_list.resize(0);
return hand_list;
}
//std::cout << " PointCloud representing the planar component: " << inliers->indices.size()
// << " data points." << std::endl;
// Extract the nonplanar inliers
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud);
extract.setIndices(inliers);
extract.setNegative(true);
std::vector<int> indices_cluster;
extract.filter(indices_cluster);
PointCloud::Ptr cloud_cluster(new PointCloud);
cloud_cluster->points.resize(indices_cluster.size());
Eigen::VectorXi cluster_cam_source(indices_cluster.size());
for (int i = 0; i < indices_cluster.size(); i++)
{
cloud_cluster->points[i] = cloud->points[indices_cluster[i]];
cluster_cam_source[i] = pts_cam_source[indices_cluster[i]];
}
cloud = cloud_cluster;
*cloud_plot = *cloud;
//std::cout << " PointCloud representing the non-planar component: " << cloud->points.size()
// << " data points." << std::endl;
}
// draw down-sampled and workspace reduced cloud
cloud_plot = cloud;
// set plotting within handle search on/off
bool plots_hands;
if (plotting_mode_ == PCL_PLOTTING)
plots_hands = true;
else
plots_hands = false;
// find hand configurations
HandSearch hand_search(finger_width_, hand_outer_diameter_, hand_depth_, hand_height_, init_bite_, num_threads_,
num_samples_, plots_hands);
hand_list = hand_search.findHands(cloud, pts_cam_source, indices, cloud_plot, calculates_antipodal, uses_clustering);
// remove hands at boundaries of workspace
if (filters_boundaries_)
{
//std::cout << "Filtering out hands close to workspace boundaries ...\n";
hand_list = filterHands(hand_list);
//std::cout << " # hands left: " << hand_list.size() << "\n";
}
double t2 = omp_get_wtime();
//std::cout << "Hand localization done in " << t2 - t0 << " sec\n";
if (plotting_mode_ == PCL_PLOTTING)
//{
plot_.plotHands(hand_list, cloud_plot, "");
//}
/*
else if (plotting_mode_ == RVIZ_PLOTTING)
{
plot_.plotGraspsRviz(hand_list, visuals_frame_);
}
*/
return hand_list;
}
