void pp_callback (const pcl::visualization::PointPickingEvent& event, void* cookie) { int idx = event.getPointIndex (); if (idx == -1) return; if (!cloud) { cloud = *reinterpret_cast<pcl::PCLPointCloud2::Ptr*> (cookie); xyzcloud.reset (new pcl::PointCloud<pcl::PointXYZ>); pcl::fromPCLPointCloud2 (*cloud, *xyzcloud); search.setInputCloud (xyzcloud); } // Return the correct index in the cloud instead of the index on the screen std::vector<int> indices (1); std::vector<float> distances (1); // Because VTK/OpenGL stores data without NaN, we lose the 1-1 correspondence, so we must search for the real point pcl::PointXYZ picked_pt; event.getPoint (picked_pt.x, picked_pt.y, picked_pt.z); //TODO: Look into this. search.nearestKSearch (picked_pt, 1, indices, distances); PCL_INFO ("Point index picked: %d (real: %d) - [%f, %f, %f]\n", idx, indices[0], picked_pt.x, picked_pt.y, picked_pt.z); idx = indices[0]; // If two points were selected, draw an arrow between them pcl::PointXYZ p1, p2; if (event.getPoints (p1.x, p1.y, p1.z, p2.x, p2.y, p2.z) && p) { std::stringstream ss; ss << p1 << p2; p->addArrow<pcl::PointXYZ, pcl::PointXYZ> (p1, p2, 1.0, 1.0, 1.0, ss.str ()); return; } // Else, if a single point has been selected std::stringstream ss; ss << idx; // Get the cloud's fields for (size_t i = 0; i < cloud->fields.size (); ++i) { if (!isMultiDimensionalFeatureField (cloud->fields[i])) continue; PCL_INFO ("Multidimensional field found: %s\n", cloud->fields[i].name.c_str ()); #if VTK_MAJOR_VERSION==6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6) ph_global.addFeatureHistogram (*cloud, cloud->fields[i].name, idx, ss.str ()); ph_global.renderOnce (); #endif } if (p) { pcl::PointXYZ pos; event.getPoint (pos.x, pos.y, pos.z); p->addText3D<pcl::PointXYZ> (ss.str (), pos, 0.0005, 1.0, 1.0, 1.0, ss.str ()); } }
int ICP::getNormal(const CloudPtr &cloud_in, NormalCloudPtr &cloud_out, pcl::search::KdTree<Point>::Ptr &tree) { tree->setInputCloud(cloud_in); pcl::NormalEstimation<Point, NormalPoint> norm_est; norm_est.setSearchMethod(tree); norm_est.setKSearch(_GetNormalKSearch); norm_est.setInputCloud(cloud_in); norm_est.compute(*cloud_out); Utils::combineField(cloud_in, cloud_out); return 0; }
void FindPickedPoint(const pcl::visualization::PointPickingEvent& event) { int idx = event.getPointIndex (); if (idx == -1) { std::cout << "Invalid pick!\n;"; return; } search.setInputCloud(build_cloud_accurate_plane); // Return the correct index in the cloud instead of the index on the screen std::vector<int> indices (1); std::vector<float> distances (1); // Because VTK/OpenGL stores data without NaN, we lose the 1-1 correspondence, so we must search for the real point pcl::PointXYZ picked_pt; event.getPoint (picked_pt.x, picked_pt.y, picked_pt.z); search.nearestKSearch (picked_pt, 1, indices, distances); picked_points.push_back(picked_pt); }
void depthPointsCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) { // Instantiate various clouds pcl::PCLPointCloud2* cloud_intermediate = new pcl::PCLPointCloud2; pcl::PCLPointCloud2ConstPtr cloudPtr(cloud_intermediate); pcl::PointCloud<pcl::PointXYZ> cloud; // Convert to PCL data type pcl_conversions::toPCL(*cloud_msg, *cloud_intermediate); // Apply Voxel Filter on PCLPointCloud2 vox.setInputCloud (cloudPtr); vox.setInputCloud (cloudPtr); vox.filter (*cloud_intermediate); // Convert PCL::PointCloud2 to PCL::PointCloud<PointXYZ> pcl::fromPCLPointCloud2(*cloud_intermediate, cloud); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p = cloud.makeShared(); // Apply Passthrough Filter pass.setFilterFieldName ("z"); pass.setFilterLimits (0.3, 1); pass.setInputCloud (cloud_p); //pass.setFilterLimitsNegative (true); pass.filter (*cloud_p); // Apply Passthrough Filter pass.setFilterFieldName ("x"); pass.setFilterLimits (-0.2, 0.2); pass.setInputCloud (cloud_p); pass.setFilterFieldName ("z"); pass.setFilterLimits (0.0, 3.0); //pass.setFilterLimitsNegative (true); pass.filter (*cloud_p); // Apply Statistical Noise Filter sor.setInputCloud (cloud_p); sor.filter (*cloud_p); // Planar segmentation: Remove large planes? Or extract floor? pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers (new pcl::PointIndices); int nr_points = (int) cloud_p->points.size (); Eigen::Vector3f lol (0, 1, 0); seg.setEpsAngle( 30.0f * (3.14f/180.0f) ); seg.setAxis(lol); //while(cloud_p->points.size () > 0.2 * nr_points) { sor.setInputCloud (cloud_p); sor.filter (*cloud_p); // Create the segmentation object pcl::SACSegmentation<pcl::PointXYZ> seg; // Optional seg.setOptimizeCoefficients (true); // Mandatory seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setDistanceThreshold (0.01); seg.setInputCloud (cloud_p); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { //break; } else { /*std::cout << "Model coefficients: " << coefficients->values[0] << " " << coefficients->values[1] << " " << coefficients->values[2] << " " << coefficients->values[3] << "\n";*/ pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud(cloud_p); extract.setIndices(inliers); extract.setNegative(true); extract.filter(*cloud_p); } //} Eigen::Vector3f lol_p (0.5f, 0, 0.5f); seg.setAxis(lol_p); while(cloud_p->points.size () > 0.1 * nr_points) { seg.setInputCloud (cloud_p); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { break; } else { /*std::cout << "Model coefficients: " << coefficients->values[0] << " " << coefficients->values[1] << " " << coefficients->values[2] << " " << coefficients->values[3] << "\n";*/ pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud(cloud_p); extract.setIndices(inliers); extract.setNegative(true); extract.filter(*cloud_p); } } // Apply Statistical Noise Filter sor.setInputCloud (cloud_p); sor.filter (*cloud_p); if(cloud_p->points.size() > 0) { std::vector<pcl::PointIndices> cluster_indices; tree->setInputCloud (cloud_p); ec.setInputCloud (cloud_p); ec.extract (cluster_indices); std::cout << "Clusters detected: " << cluster_indices.size() << "\n"; // Convert to ROS data type } // Convert to ROS data type sensor_msgs::PointCloud2 output; pcl::toROSMsg(*cloud_p, output); // Publish the data downsample_pub.publish(output); }