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; }