std::vector<GraspHypothesis> HandSearch::findHands(const PointCloud::Ptr cloud,
const Eigen::VectorXi& pts_cam_source, const std::vector<int>& indices,
const PointCloud::Ptr cloud_plot, bool calculates_antipodal,
bool uses_clustering)
{
// create KdTree for neighborhood search
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud(cloud);
cloud_normals_.resize(3, cloud->size());
cloud_normals_.setZero(3, cloud->size());
// calculate normals for all points
if (calculates_antipodal)
{
//std::cout << "Calculating normals for all points\n";
nn_radius_taubin_ = 0.01;
std::vector<int> indices_cloud(cloud->size());
for (int i = 0; i < indices_cloud.size(); i++)
indices_cloud[i] = i;
findQuadrics(cloud, pts_cam_source, kdtree, indices_cloud);
nn_radius_taubin_ = 0.03;
}
// draw samples from the point cloud uniformly
std::vector<int> indices_rand;
Eigen::VectorXi hands_cam_source;
if (indices.size() == 0)
{
double t_rand = omp_get_wtime();
//std::cout << "Generating uniform random indices ...\n";
indices_rand.resize(num_samples_);
pcl::RandomSample<pcl::PointXYZ> random_sample;
random_sample.setInputCloud(cloud);
random_sample.setSample(num_samples_);
random_sample.filter(indices_rand);
hands_cam_source.resize(num_samples_);
for (int i = 0; i < num_samples_; i++)
hands_cam_source(i) = pts_cam_source(indices_rand[i]);
//std::cout << " Done in " << omp_get_wtime() - t_rand << " sec\n";
}
else
indices_rand = indices;
if (plots_samples_)
plot_.plotSamples(indices_rand, cloud);
// find quadrics
//std::cout << "Estimating local axes ...\n";
std::vector<Quadric> quadric_list = findQuadrics(cloud, pts_cam_source, kdtree, indices_rand);
if (plots_local_axes_)
plot_.plotLocalAxes(quadric_list, cloud_plot);
// find hands
//std::cout << "Finding hand poses ...\n";
std::vector<GraspHypothesis> hand_list = findHands(cloud, pts_cam_source, quadric_list, hands_cam_source, kdtree);
return hand_list;
}
bool RegionGrowing::seedRegionGrowing(
pcl::PointCloud<PointNormalT>::Ptr src_points,
const PointT seed_point, const PointCloud::Ptr cloud,
PointNormal::Ptr normals) {
if (cloud->empty() || normals->size() != cloud->size()) {
ROS_ERROR("- Region growing failed. Incorrect inputs sizes ");
return false;
}
if (isnan(seed_point.x) || isnan(seed_point.y) || isnan(seed_point.z)) {
ROS_ERROR("- Seed Point is Nan. Skipping");
return false;
}
this->kdtree_->setInputCloud(cloud);
std::vector<int> neigbor_indices;
this->getPointNeigbour<int>(neigbor_indices, seed_point, 1);
int seed_index = neigbor_indices[0];
const int in_dim = static_cast<int>(cloud->size());
int *labels = reinterpret_cast<int*>(malloc(sizeof(int) * in_dim));
#ifdef _OPENMP
#pragma omp parallel for num_threads(this->num_threads_)
#endif
for (int i = 0; i < in_dim; i++) {
if (i == seed_index) {
labels[i] = 1;
}
labels[i] = -1;
}
this->seedCorrespondingRegion(labels, cloud, normals,
seed_index, seed_index);
src_points->clear();
for (int i = 0; i < in_dim; i++) {
if (labels[i] != -1) {
PointNormalT pt;
pt.x = cloud->points[i].x;
pt.y = cloud->points[i].y;
pt.z = cloud->points[i].z;
pt.r = cloud->points[i].r;
pt.g = cloud->points[i].g;
pt.b = cloud->points[i].b;
pt.normal_x = normals->points[i].normal_x;
pt.normal_y = normals->points[i].normal_y;
pt.normal_z = normals->points[i].normal_z;
src_points->push_back(pt);
}
}
free(labels);
return true;
}
void WorldDownloadManager::pclPointCloudToMessage(PointCloud::Ptr pcl_cloud,
std::vector<kinfu_msgs::KinfuCloudPoint> & message)
{
uint cloud_size = pcl_cloud->size();
message.resize(cloud_size);
for (uint i = 0; i < cloud_size; i++)
{
message[i].x = (*pcl_cloud)[i].x;
message[i].y = (*pcl_cloud)[i].y;
message[i].z = (*pcl_cloud)[i].z;
}
}
void WorldDownloadManager::cropMesh(const kinfu_msgs::KinfuCloudPoint & min,
const kinfu_msgs::KinfuCloudPoint & max,PointCloud::ConstPtr cloud,
TrianglesConstPtr triangles,PointCloud::Ptr out_cloud,TrianglesPtr out_triangles)
{
const uint triangles_size = triangles->size();
const uint cloud_size = cloud->size();
std::vector<bool> valid_points(cloud_size,true);
std::vector<uint> valid_points_remap(cloud_size,0);
std::cout << "Starting with " << cloud_size << " points and " << triangles_size << " triangles.\n";
uint offset;
// check the points
for (uint i = 0; i < cloud_size; i++)
{
const pcl::PointXYZ & pt = (*cloud)[i];
if (pt.x > max.x || pt.y > max.y || pt.z > max.z ||
pt.x < min.x || pt.y < min.y || pt.z < min.z)
valid_points[i] = false;
}
// discard invalid points
out_cloud->clear();
out_cloud->reserve(cloud_size);
offset = 0;
for (uint i = 0; i < cloud_size; i++)
if (valid_points[i])
{
out_cloud->push_back((*cloud)[i]);
// save new position for triangles remap
valid_points_remap[i] = offset;
offset++;
}
out_cloud->resize(offset);
// discard invalid triangles
out_triangles->clear();
out_triangles->reserve(triangles_size);
offset = 0;
for (uint i = 0; i < triangles_size; i++)
{
const kinfu_msgs::KinfuMeshTriangle & tri = (*triangles)[i];
bool is_valid = true;
// validate all the vertices
for (uint h = 0; h < 3; h++)
if (!valid_points[tri.vertex_id[h]])
{
is_valid = false;
break;
}
if (is_valid)
{
kinfu_msgs::KinfuMeshTriangle out_tri;
// remap the triangle
for (uint h = 0; h < 3; h++)
out_tri.vertex_id[h] = valid_points_remap[(*triangles)[i].vertex_id[h]];
out_triangles->push_back(out_tri);
offset++;
}
}
out_triangles->resize(offset);
std::cout << "Ended with " << out_cloud->size() << " points and " << out_triangles->size() << " triangles.\n";
}
void RegionGrowing::fastSeedRegionGrowing(
pcl::PointCloud<PointNormalT>::Ptr src_points,
cv::Point2i &seed_index2D, const PointCloud::Ptr cloud,
const PointNormal::Ptr normals, const PointT seed_pt) {
if (cloud->empty() || normals->size() != cloud->size()) {
return;
}
cv::Point2f image_index;
int seed_index = -1;
if (this->projectPoint3DTo2DIndex(image_index, seed_pt)) {
seed_index = (static_cast<int>(image_index.x) +
(static_cast<int>(image_index.y) * camera_info_->width));
seed_index2D = cv::Point2i(static_cast<int>(image_index.x),
static_cast<int>(image_index.y));
} else {
ROS_ERROR("INDEX IS NAN");
return;
}
#ifdef _DEBUG
cv::Mat test = cv::Mat::zeros(480, 640, CV_8UC3);
cv::circle(test, image_index, 3, cv::Scalar(0, 255, 0), -1);
cv::imshow("test", test);
cv::waitKey(3);
#endif
Eigen::Vector4f seed_point = cloud->points[seed_index].getVector4fMap();
Eigen::Vector4f seed_normal = normals->points[
seed_index].getNormalVector4fMap();
std::vector<int> processing_list;
std::vector<int> labels(static_cast<int>(cloud->size()), -1);
const int window_size = 3;
const int wsize = window_size * window_size;
const int lenght = std::floor(window_size/2);
processing_list.clear();
for (int j = -lenght; j <= lenght; j++) {
for (int i = -lenght; i <= lenght; i++) {
int index = (seed_index + (j * camera_info_->width)) + i;
if (index >= 0 && index < cloud->size()) {
processing_list.push_back(index);
}
}
}
std::vector<int> temp_list;
while (true) {
if (processing_list.empty()) {
break;
}
temp_list.clear();
for (int i = 0; i < processing_list.size(); i++) {
int idx = processing_list[i];
if (labels[idx] == -1) {
Eigen::Vector4f c = cloud->points[idx].getVector4fMap();
Eigen::Vector4f n = normals->points[idx].getNormalVector4fMap();
if (this->seedVoxelConvexityCriteria(
seed_point, seed_normal, seed_point, c, n, -0.01) == 1) {
labels[idx] = 1;
for (int j = -lenght; j <= lenght; j++) {
for (int k = -lenght; k <= lenght; k++) {
int index = (idx + (j * camera_info_->width)) + k;
if (index >= 0 && index < cloud->size()) {
temp_list.push_back(index);
}
}
}
}
}
}
processing_list.clear();
processing_list.insert(processing_list.end(), temp_list.begin(),
temp_list.end());
}
src_points->clear();
for (int i = 0; i < labels.size(); i++) {
if (labels[i] != -1) {
PointNormalT pt;
pt.x = cloud->points[i].x;
pt.y = cloud->points[i].y;
pt.z = cloud->points[i].z;
pt.r = cloud->points[i].r;
pt.g = cloud->points[i].g;
pt.b = cloud->points[i].b;
pt.normal_x = normals->points[i].normal_x;
pt.normal_y = normals->points[i].normal_y;
pt.normal_z = normals->points[i].normal_z;
src_points->push_back(pt);
}
}
}
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<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;
}